1/*
2 * AFCS_MODEL1_sf.c
3 *
4 * Code generation for model "AFCS_MODEL1_sf".
5 *
6 * Model version : 1.995
7 * Simulink Coder version : 8.10 (R2016a) 10-Feb-2016
8 * C source code generated on : Wed Aug 11 17:17:39 2021
9 *
10 * Target selection: rtwsfcn.tlc
11 * Note: GRT includes extra infrastructure and instrumentation for prototyping
12 * Embedded hardware selection: ARM Compatible->ARM Cortex
13 * Emulation hardware selection:
14 * Differs from embedded hardware (MATLAB Host)
15 * Code generation objectives: Unspecified
16 * Validation result: Not run
17 */
18
19#include <math.h>
20#include "AFCS_MODEL1_sf.h"
21#include "AFCS_MODEL1_sf_private.h"
22#undef S_FUNCTION_NAME
23#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_StickGradientDeadZone_sf)
24#define RTW_GENERATED_SFCN_TUNABLE_PRMS_StickGradientDeadZone_sf
25#endif
26
27#include "StickGradientDeadZone_sf.h"
28#undef S_FUNCTION_NAME
29#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_PositivePass_sf)
30#define RTW_GENERATED_SFCN_TUNABLE_PRMS_PositivePass_sf
31#endif
32
33#include "PositivePass_sf.h"
34#undef S_FUNCTION_NAME
35#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_NegativePass_sf)
36#define RTW_GENERATED_SFCN_TUNABLE_PRMS_NegativePass_sf
37#endif
38
39#include "NegativePass_sf.h"
40#undef S_FUNCTION_NAME
41#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_WashoutFilter_sf)
42#define RTW_GENERATED_SFCN_TUNABLE_PRMS_WashoutFilter_sf
43#endif
44
45#include "WashoutFilter_sf.h"
46#undef S_FUNCTION_NAME
47#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_RateLimiter_sf)
48#define RTW_GENERATED_SFCN_TUNABLE_PRMS_RateLimiter_sf
49#endif
50
51#include "RateLimiter_sf.h"
52#undef S_FUNCTION_NAME
53#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_Fader_sf)
54#define RTW_GENERATED_SFCN_TUNABLE_PRMS_Fader_sf
55#endif
56
57#include "Fader_sf.h"
58#undef S_FUNCTION_NAME
59#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_Res180_sf)
60#define RTW_GENERATED_SFCN_TUNABLE_PRMS_Res180_sf
61#endif
62
63#include "Res180_sf.h"
64#undef S_FUNCTION_NAME
65#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_IntegratorLimited_sf)
66#define RTW_GENERATED_SFCN_TUNABLE_PRMS_IntegratorLimited_sf
67#endif
68
69#include "IntegratorLimited_sf.h"
70#undef S_FUNCTION_NAME
71#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_LeadLagFilter_sf)
72#define RTW_GENERATED_SFCN_TUNABLE_PRMS_LeadLagFilter_sf
73#endif
74
75#include "LeadLagFilter_sf.h"
76#undef S_FUNCTION_NAME
77#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_Gain_sf)
78#define RTW_GENERATED_SFCN_TUNABLE_PRMS_Gain_sf
79#endif
80
81#include "Gain_sf.h"
82#undef S_FUNCTION_NAME
83#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_SaturationLimiter_sf)
84#define RTW_GENERATED_SFCN_TUNABLE_PRMS_SaturationLimiter_sf
85#endif
86
87#include "SaturationLimiter_sf.h"
88#undef S_FUNCTION_NAME
89#if !defined(RTW_GENERATED_SFCN_TUNABLE_PRMS_LagFilter_sf)
90#define RTW_GENERATED_SFCN_TUNABLE_PRMS_LagFilter_sf
91#endif
92
93#include "LagFilter_sf.h"
94#undef S_FUNCTION_NAME
95#define S_FUNCTION_NAME AFCS_MODEL1_sf
96#include "simstruc.h"
97#include "fixedpoint.h"
98
99/* Named constants for Chart: '<S21>/delayblock1' */
100#define AFCS_MODEL1_IN_Input0 ((uint8_T)1U)
101#define AFCS_MODEL1_IN_Input1 ((uint8_T)2U)
102#define AFCS_MODEL1_IN_NO_ACTIVE_CHILD ((uint8_T)0U)
103#define AFCS_MODEL1_IN_Output0 ((uint8_T)3U)
104#define AFCS_MODEL1_IN_Output1 ((uint8_T)4U)
105#define AFCS_MODEL1_IN_delayblock ((uint8_T)1U)
106
107/* Named constants for Chart: '<S299>/AirLineUpdateSucModel' */
108#define AFCS_MODEL1_IN_Alt1 ((uint8_T)2U)
109#define AFCS_MODEL1_IN_pre ((uint8_T)3U)
110#define AFCS_M_IN_AirLineUpdateSucModel ((uint8_T)1U)
111#define AFCS__IN_AirLineUpdateSucModel1 ((uint8_T)1U)
112
113/* Named constants for Chart: '<S24>/zeros2' */
114#define AFCS_MODEL1_IN_zero ((uint8_T)1U)
115
116/* Named constants for Chart: '<S29>/LLACalibration2' */
117#define AFCS_MODEL1_IN_Alt1_o ((uint8_T)1U)
118#define AFCS_MODEL1_IN_Alt2 ((uint8_T)2U)
119#define AFCS_MODEL1_IN_AltCalibration ((uint8_T)1U)
120
121/* Named constants for Chart: '<S319>/Pre_Takeoff' */
122#define AFCS_MODEL1_IN_LockState ((uint8_T)1U)
123#define AFCS_MODEL1_IN_Pre_Takeoff ((uint8_T)1U)
124#define AFCS_MODEL1_IN_StickCheck ((uint8_T)2U)
125#define AFCS_MODEL1_IN_TransTakeoff ((uint8_T)3U)
126
127/* Named constants for Chart: '<S321>/Takeoff' */
128#define AFCS_MODEL1_IN_AbortTakeoff ((uint8_T)1U)
129#define AFCS_MODEL1_IN_DrControl ((uint8_T)1U)
130#define AFCS_MODEL1_IN_Initial ((uint8_T)2U)
131#define AFCS_MODEL1_IN_LiftFrontWheel ((uint8_T)3U)
132#define AFCS_MODEL1_IN_NormalTakeoff ((uint8_T)2U)
133#define AFCS_MODEL1_IN_NwsControl ((uint8_T)4U)
134#define AFCS_MODEL1_IN_StartLatControl ((uint8_T)5U)
135#define AFCS_MODEL1_IN_Takeoff ((uint8_T)1U)
136#define AFCS_MODEL1_IN_TransClimb ((uint8_T)6U)
137
138/* Named constants for Chart: '<S314>/Climb2' */
139#define AFCS_MODEL1_IN_Climb ((uint8_T)1U)
140#define AFCS_MODEL1_IN_PhiControl ((uint8_T)1U)
141#define AFCS_MODEL1_IN_PositionControl ((uint8_T)1U)
142#define AFCS_MODEL1_IN_PositionControl1 ((uint8_T)1U)
143#define AFCS_MODEL1_IN_TransCruise ((uint8_T)2U)
144#define AFCS_MODEL1_IN_YrwControl ((uint8_T)2U)
145#define AFCS_MODEL1_IN_YrwControl_o ((uint8_T)1U)
146#define AFCS_MODEL1_IN_phiLcontrol ((uint8_T)2U)
147
148/* Named constants for Chart: '<S315>/Cruise' */
149#define AFCS_MODEL1_IN_AT_MODE ((uint8_T)1U)
150#define AFCS_MODEL1_IN_AltControl ((uint8_T)1U)
151#define AFCS_MODEL1_IN_Cruise ((uint8_T)1U)
152#define AFCS_MODEL1_IN_Hcontrol ((uint8_T)1U)
153#define AFCS_MODEL1_IN_HdotControl ((uint8_T)2U)
154#define AFCS_MODEL1_IN_PhiControl1 ((uint8_T)2U)
155#define AFCS_MODEL1_IN_ReFly_MODE ((uint8_T)3U)
156#define AFCS_MODEL1_IN_YD_MODE ((uint8_T)4U)
157#define AFCS_MODEL1_IN_gamacontrol ((uint8_T)2U)
158#define AFCS_MODEL1_IN_gamacontrol1 ((uint8_T)3U)
159#define AFCS_MODEL1_IN_thrustControl1 ((uint8_T)2U)
160#define AFCS_MODEL_IN_GPSFailPhiControl ((uint8_T)1U)
161
162/* Named constants for Chart: '<S313>/Approach2' */
163#define AFCS_MODEL1_IN_Approach ((uint8_T)1U)
164#define AFCS_MODEL1_IN_EmYrwControl ((uint8_T)1U)
165#define AFCS_MODEL1_IN_FlightMode ((uint8_T)1U)
166#define AFCS_MODEL1_IN_LevelPosition ((uint8_T)2U)
167#define AFCS_MODEL1_IN_NormYrwControl ((uint8_T)2U)
168#define AFCS_MODEL1_IN_PhiLControl3 ((uint8_T)3U)
169#define AFCS_MODEL1_IN_TransLand ((uint8_T)2U)
170#define AFCS_MODEL1_IN_TranseNorm ((uint8_T)4U)
171#define AFCS_MODEL1_IN_YrwControl1 ((uint8_T)2U)
172#define AFCS_MODEL1_IN_phiLcontrol2 ((uint8_T)2U)
173
174/* Named constants for Chart: '<S316>/Landing2' */
175#define AFCS_MODEL1_IN_Decline ((uint8_T)1U)
176#define AFCS_MODEL1_IN_FlightMode_e ((uint8_T)2U)
177#define AFCS_MODEL1_IN_GamaControl1 ((uint8_T)2U)
178#define AFCS_MODEL1_IN_GamaControl2 ((uint8_T)3U)
179#define AFCS_MODEL1_IN_GoAround ((uint8_T)4U)
180#define AFCS_MODEL1_IN_Landing ((uint8_T)1U)
181#define AFCS_MODEL1_IN_LeftSpiral10s ((uint8_T)3U)
182#define AFCS_MODEL1_IN_PhiLontrol3 ((uint8_T)2U)
183#define AFCS_MODEL1_IN_Pre_Land ((uint8_T)5U)
184#define AFCS_MODEL1_IN_TransLand_o ((uint8_T)4U)
185#define AFCS_MODEL1_IN_TranseNorm_c ((uint8_T)3U)
186#define AFCS_MODEL1_IN_VupControl3 ((uint8_T)6U)
187#define AFCS_MODEL1_IN_YrwControl11 ((uint8_T)4U)
188
189/* Named constants for Chart: '<S322>/Taxi' */
190#define AFCS_MODEL1_IN_EndControl ((uint8_T)2U)
191#define AFCS_MODEL1_IN_NoDrControl ((uint8_T)3U)
192#define AFCS_MODEL1_IN_Taxi ((uint8_T)1U)
193#define AFCS_MODEL1_IN_ThetaControl ((uint8_T)5U)
194
195/* Named constants for Chart: '<S32>/wowcmp1' */
196#define AFCS_MODEL1_IN_Enlowfg ((uint8_T)1U)
197#define AFCS_MODEL1_IN_count ((uint8_T)1U)
198#define AFCS_MODEL1_IN_enableFG ((uint8_T)2U)
199#define AFCS_MODEL1_IN_enableFG1 ((uint8_T)3U)
200#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)
201
202extern void *AFCS_MODEL1_malloc(SimStruct *S);
203
204#endif
205
206#ifndef __RTW_UTFREE__
207#if defined (MATLAB_MEX_FILE)
208
209extern void * utMalloc(size_t);
210extern void utFree(void *);
211
212#endif
213#endif /* #ifndef __RTW_UTFREE__ */
214
215/* Forward declaration for local functions */
216static real_T AFCS_MODEL1_rt_roundd_snf(real_T u);
217static real_T AFCS_MODEL1_rt_remd_snf(real_T u0, real_T u1);
218static void AFCS_MODEL1_sind_m(real_T *x);
219static void AFCS_MODEL1_cosd_c(real_T *x);
220static real_T AFCS_MODEL1_rt_atan2d_snf(real_T u0, real_T u1);
221static real_T AFCS_MODEL1_rt_modd_snf(real_T u0, real_T u1);
222static real32_T AFCS_MODEL1_rt_atan2f_snf(real32_T u0, real32_T u1);
223static int32_T AFCS_MODEL1_addup_g(int32_T bA, int32_T iB, int32_T iC);
224static void AFCS_MODEL1_GetPsi(real_T A_lon, real_T A_lat, real_T B_lon, real_T
225 B_lat, real_T *Drange, real_T *PsiNorth);
226static real_T AFCS_MODEL1_rt_powd_snf(real_T u0, real_T u1);
227static int32_T AFCS_MODEL1_addup_l(int32_T bA, int32_T iB, int32_T iC);
228static int32_T AFCS_MODEL1_addup_p(boolean_T bA, int32_T iB, int32_T iC);
229static int32_T AFCS_MODEL1_addup_i(int32_T bA, int32_T iB, int32_T iC);
230static int32_T AFCS_MODEL1_addup_k(int32_T bA, int32_T iB, int32_T iC);
231static void AFCS_MODEL1_TransformLLATBDX(real_T Lon, real_T Lat, real_T Alt,
232 real_T Lon0, real_T Lat0, real_T Alt0, real_T *pn, real_T *pe, real_T *pd);
233static void AFCS_MODEL1_linspace(real_T y[32]);
234static int32_T AFCS_MODEL1_addup_h(int32_T bA, int32_T iB, int32_T iC);
235static real32_T AFCS_MODEL1_rt_roundf_snf(real32_T u);
236static real32_T AFCS_MODEL1_rt_remf_snf(real32_T u0, real32_T u1);
237static int32_T AFCS_MODEL1_addup_lc(boolean_T bA, int32_T iB, int32_T iC);
238static void AFCS_MODEL1_NormalTakeoff(SimStruct *S);
239static int32_T AFCS_MODEL1_addup_jz(boolean_T bA, int32_T iB, int32_T iC);
240static real32_T AFCS_MODEL1_res180_m(real32_T angle);
241static void AFCS_MODEL1_Pre_Land(SimStruct *S);
242static void AFCS_MODE_exit_internal_Landing(SimStruct *S);
243static void AFCS_MODEL1_XYPositionControl_d(SimStruct *S);
244static void AFCS_MODEL1_Landing(SimStruct *S);
245static int32_T AFCS_MODEL1_addup_od(boolean_T bA, int32_T iB, int32_T iC);
246static int32_T AFCS_MODEL1_addup_ke(int8_T bA, int32_T iB, int32_T iC);
247static int32_T AFCS_MODEL1_addup_j(boolean_T bA, int32_T iB, int32_T iC);
248static int32_T AFCS_MODEL1_addup_f(boolean_T bA, int32_T iB, int32_T iC);
249static real32_T AFCS_MODEL1_res180_l(real32_T angle);
250static void AFCS_MODEL1_XYPositionControl(SimStruct *S);
251static void AFCS_MODEL1_tand(real_T *x);
252static real32_T AFCS_MODEL1_res180(real32_T angle);
253static int32_T AFCS_MODEL1_addup_o(boolean_T bA, int32_T iB, int32_T iC);
254static void AFCS_MODEL1_PGT_CalculateRmin(const real_T xValues[32], const real_T
255 yValues[32], int8_T IndataLength, real_T Rmin[32], real_T b_RminNew[255]);
256static void rt_mrdivide_U1d1x3_U2d_9vOrDY_k(const real_T u0[3], const real_T u1
257 [9], real_T y[3]);
258static void AFCS_MODEL1_PGT_changewaypoints(const real_T xValues[32], const
259 real_T yValues[32], const real_T hValues[32], int8_T IndataLength, const
260 real_T Rmin[255], const real_T vValues[32], real_T vx_qiedian_out[255], real_T
261 vy_qiedian_out[255], real_T xn_qiedian_out[255], real_T ye_qiedian_out[255],
262 real_T zh_qiedian_out[255]);
263static int32_T AFCS_MODEL1_addup(int32_T bA, int32_T iB, int32_T iC);
264static void AFCS_MODEL1_CompDistance(real32_T X1, real32_T Y1, real32_T X2,
265 real32_T Y2, real32_T Xac, real32_T Yac, real32_T *DD, real32_T *A, real32_T
266 *Xrw, real32_T *Yrw);
267static int32_T AFCS_MODEL1_addup_m(int32_T bA, int32_T iB, int32_T iC);
268static void AFCS_MODEL1_cosd(real32_T *x);
269static void PGT_followOrbit_nonlinear_guida(real_T cn, real_T ce, real_T roup,
270 real_T lamda, real_T pn, real_T pe, real_T vn, real_T ve, real_T psi, real_T
271 zz, real_T nav_param, real_T *hc, real_T *psic, real_T *acmd);
272static void AFCS_MODEL1_sind(real32_T *x);
273static void AFCS_MODEL1_cosd_i(real_T *x);
274static void AFCS_MODEL1_sind_k(real_T *x);
275static void AFCS_MODEL1_PGT_circledot(real_T x, real_T y, real_T b_vx, real_T
276 b_vy, real_T r, real_T *x_dot, real_T *y_dot, real_T *fi);
277static void AFCS_MODEL1_PGT_Dubins_2D(real_T x1, real_T b_y1, real_T v1x, real_T
278 v1y, real_T x2, real_T y2, real_T v2x, real_T v2y, real_T r_min, real_T *xq1,
279 real_T *yq1, real_T *fis, real_T *fi1, real_T *xq2, real_T *yq2, real_T *fif,
280 real_T *fi2, real_T *L, real_T *A, real_T *B, real_T *C, real_T *x0_dot,
281 real_T *y0_dot, real_T *tx_dot, real_T *ty_dot);
282static void AFCS_MODEL1_PGT_circledot_b(real_T x, real_T y, real_T b_vx, real_T
283 b_vy, real_T r, real_T *x_dot, real_T *y_dot, real_T *fi);
284static void AFCS_MODEL1_PGT_Dubins_2D_d(real_T x1, real_T b_y1, real_T v1x,
285 real_T v1y, real_T x2, real_T y2, real_T v2x, real_T v2y, real_T r_min, real_T
286 *xq1, real_T *yq1, real_T *fis, real_T *fi1, real_T *xq2, real_T *yq2, real_T *
287 fif, real_T *fi2, real_T *L, real_T *A, real_T *B, real_T *C, real_T *x0_dot,
288 real_T *y0_dot, real_T *tx_dot, real_T *ty_dot);
289static void AFCS_MODEL1_PGT_Dubins_2Dmin(real_T x1, real_T b_y1, real_T v1x,
290 real_T v1y, real_T x2, real_T y2, real_T v2x, real_T v2y, real_T r_min, real_T
291 *xq1, real_T *yq1, real_T *fis, real_T *fi1, real_T *Lamda1, real_T *xq2,
292 real_T *yq2, real_T *fif, real_T *fi2, real_T *Lamda2, real_T *L, real_T
293 *x0_dot, real_T *y0_dot, real_T *tx_dot, real_T *ty_dot);
294static void AFCS_MODE_PGT_Guidance3D_Dubins(real_T xs, real_T ys, real_T hs,
295 real_T vxs, real_T vys, real_T xe, real_T ye, real_T he, real_T vxe, real_T
296 vye, real_T Rmin, real_T *R, real_T *Gama, real_T *xq1, real_T *yq1, real_T
297 *hq1, real_T *fis, real_T *Lamda1, real_T *xq2, real_T *yq2, real_T *hq2,
298 real_T *fi2, real_T *Lamda2, real_T *x0_dot, real_T *y0_dot, real_T *tx_dot,
299 real_T *ty_dot, real_T *fi1sum, real_T *fi2sum);
300
301#if defined(MATLAB_MEX_FILE)
302#include "rt_nonfinite.c"
303#endif
304
305static const char_T *RT_MEMORY_ALLOCATION_ERROR =
306 "memory allocation error in generated S-Function";
307
308/* Lookup 1D UtilityAFCS_MODEL1_sf_LookUpE_4Ol9QRGv */
309void AFCS_MODEL1_sf_LookUpE_4Ol9QRGv(real_T *pY, const real_T *pYData, real_T u,
310 real_T valueLo, uint32_T iHi, real_T uSpacing)
311{
312 if (u <= valueLo ) {
313 (*pY) = (*pYData);
314 } else {
315 real_T uAdjusted = u - valueLo;
316 real_T tmpIdxLeft = uAdjusted / uSpacing;
317 uint32_T iLeft = (uint32_T)tmpIdxLeft;
318 if ((tmpIdxLeft >= 4294967296.0) || (iLeft >= iHi) ) {
319 (*pY) = pYData[iHi];
320 } else {
321 {
322 real_T lambda;
323
324 {
325 real_T num = (real_T)uAdjusted - ( iLeft * uSpacing );
326 lambda = num / uSpacing;
327 }
328
329 {
330 real_T yLeftCast;
331 real_T yRghtCast;
332 yLeftCast = pYData[iLeft];
333 yRghtCast = pYData[((iLeft)+1)];
334 yLeftCast += lambda * ( yRghtCast - yLeftCast );
335 (*pY) = yLeftCast;
336 }
337 }
338 }
339 }
340}
341
342/*
343 * Taylor Series expansion approximation of WGS84 model of
344 * ellipsoid normal gravity
345 */
346void wgs84_taylor_series(real_T *h, real_T *phi, real_T opt_m2ft, real_T *y,
347 int_T k)
348{
349 real_T gamma_ts, m, sinphi, sin2phi;
350 int_T i;
351 for (i = 0; i < k; i++ ) {
352 sinphi = sin(phi[i]);
353 sin2phi = sinphi*sinphi;
354
355 /* Calculate theoretical normal gravity (gamma) /eq. 4-1/ */
356 gamma_ts = (WGS84_G_E)*( 1.0 + (WGS84_K)*sin2phi )/( sqrt(1.0 - (WGS84_E_2)*
357 sin2phi) );
358 m = (WGS84_A)*(WGS84_A)*(WGS84_B)*(WGS84_W_DEF)*(WGS84_W_DEF)/
359 (WGS84_GM_DEF);
360
361 /* Return normal gravity as the output /eq. 4-3/ */
362 y[i] = opt_m2ft*gamma_ts*( 1.0 - 2.0*( 1.0 + 1.0/(WGS84_INV_F) + m -
363 2.0*sin2phi/(WGS84_INV_F) )*h[i]/(WGS84_A) + 3.0*h[i]*h[i]/(
364 (WGS84_A)*(WGS84_A)) );
365 }
366}
367
368real32_T AFCS_MODEL1_look1_iflf_binlcpw(real32_T u0, const real32_T bp0[], const
369 real32_T table[], uint32_T maxIndex)
370{
371 real32_T frac;
372 uint32_T iRght;
373 uint32_T iLeft;
374 uint32_T bpIdx;
375
376 /* Lookup 1-D
377 Search method: 'binary'
378 Use previous index: 'off'
379 Interpolation method: 'Linear'
380 Extrapolation method: 'Clip'
381 Use last breakpoint for index at or above upper limit: 'off'
382 Remove protection against out-of-range input in generated code: 'off'
383 */
384 /* Prelookup - Index and Fraction
385 Index Search method: 'binary'
386 Extrapolation method: 'Clip'
387 Use previous index: 'off'
388 Use last breakpoint for index at or above upper limit: 'off'
389 Remove protection against out-of-range input in generated code: 'off'
390 */
391 if (u0 <= bp0[0U]) {
392 iLeft = 0U;
393 frac = 0.0F;
394 } else if (u0 < bp0[maxIndex]) {
395 /* Binary Search */
396 bpIdx = maxIndex >> 1U;
397 iLeft = 0U;
398 iRght = maxIndex;
399 while (iRght - iLeft > 1U) {
400 if (u0 < bp0[bpIdx]) {
401 iRght = bpIdx;
402 } else {
403 iLeft = bpIdx;
404 }
405
406 bpIdx = (iRght + iLeft) >> 1U;
407 }
408
409 frac = (u0 - bp0[iLeft]) / (bp0[iLeft + 1U] - bp0[iLeft]);
410 } else {
411 iLeft = maxIndex - 1U;
412 frac = 1.0F;
413 }
414
415 /* Interpolation 1-D
416 Interpolation method: 'Linear'
417 Use last breakpoint for index at or above upper limit: 'off'
418 Overflow mode: 'portable wrapping'
419 */
420 return (table[iLeft + 1U] - table[iLeft]) * frac + table[iLeft];
421}
422
423real32_T AFCS_MODEL1_look1_iflf_binlxpw(real32_T u0, const real32_T bp0[], const
424 real32_T table[], uint32_T maxIndex)
425{
426 real32_T frac;
427 uint32_T iRght;
428 uint32_T iLeft;
429 uint32_T bpIdx;
430
431 /* Lookup 1-D
432 Search method: 'binary'
433 Use previous index: 'off'
434 Interpolation method: 'Linear'
435 Extrapolation method: 'Linear'
436 Use last breakpoint for index at or above upper limit: 'off'
437 Remove protection against out-of-range input in generated code: 'off'
438 */
439 /* Prelookup - Index and Fraction
440 Index Search method: 'binary'
441 Extrapolation method: 'Linear'
442 Use previous index: 'off'
443 Use last breakpoint for index at or above upper limit: 'off'
444 Remove protection against out-of-range input in generated code: 'off'
445 */
446 if (u0 <= bp0[0U]) {
447 iLeft = 0U;
448 frac = (u0 - bp0[0U]) / (bp0[1U] - bp0[0U]);
449 } else if (u0 < bp0[maxIndex]) {
450 /* Binary Search */
451 bpIdx = maxIndex >> 1U;
452 iLeft = 0U;
453 iRght = maxIndex;
454 while (iRght - iLeft > 1U) {
455 if (u0 < bp0[bpIdx]) {
456 iRght = bpIdx;
457 } else {
458 iLeft = bpIdx;
459 }
460
461 bpIdx = (iRght + iLeft) >> 1U;
462 }
463
464 frac = (u0 - bp0[iLeft]) / (bp0[iLeft + 1U] - bp0[iLeft]);
465 } else {
466 iLeft = maxIndex - 1U;
467 frac = (u0 - bp0[maxIndex - 1U]) / (bp0[maxIndex] - bp0[maxIndex - 1U]);
468 }
469
470 /* Interpolation 1-D
471 Interpolation method: 'Linear'
472 Use last breakpoint for index at or above upper limit: 'off'
473 Overflow mode: 'portable wrapping'
474 */
475 return (table[iLeft + 1U] - table[iLeft]) * frac + table[iLeft];
476}
477
478real_T AFCS_MODEL1_look1_binlxpw(real_T u0, const real_T bp0[], const real_T
479 table[], uint32_T maxIndex)
480{
481 real_T frac;
482 uint32_T iRght;
483 uint32_T iLeft;
484 uint32_T bpIdx;
485
486 /* Lookup 1-D
487 Search method: 'binary'
488 Use previous index: 'off'
489 Interpolation method: 'Linear'
490 Extrapolation method: 'Linear'
491 Use last breakpoint for index at or above upper limit: 'off'
492 Remove protection against out-of-range input in generated code: 'off'
493 */
494 /* Prelookup - Index and Fraction
495 Index Search method: 'binary'
496 Extrapolation method: 'Linear'
497 Use previous index: 'off'
498 Use last breakpoint for index at or above upper limit: 'off'
499 Remove protection against out-of-range input in generated code: 'off'
500 */
501 if (u0 <= bp0[0U]) {
502 iLeft = 0U;
503 frac = (u0 - bp0[0U]) / (bp0[1U] - bp0[0U]);
504 } else if (u0 < bp0[maxIndex]) {
505 /* Binary Search */
506 bpIdx = maxIndex >> 1U;
507 iLeft = 0U;
508 iRght = maxIndex;
509 while (iRght - iLeft > 1U) {
510 if (u0 < bp0[bpIdx]) {
511 iRght = bpIdx;
512 } else {
513 iLeft = bpIdx;
514 }
515
516 bpIdx = (iRght + iLeft) >> 1U;
517 }
518
519 frac = (u0 - bp0[iLeft]) / (bp0[iLeft + 1U] - bp0[iLeft]);
520 } else {
521 iLeft = maxIndex - 1U;
522 frac = (u0 - bp0[maxIndex - 1U]) / (bp0[maxIndex] - bp0[maxIndex - 1U]);
523 }
524
525 /* Interpolation 1-D
526 Interpolation method: 'Linear'
527 Use last breakpoint for index at or above upper limit: 'off'
528 Overflow mode: 'portable wrapping'
529 */
530 return (table[iLeft + 1U] - table[iLeft]) * frac + table[iLeft];
531}
532
533real_T AFCS_MODEL1_look2_binlxpw(real_T u0, real_T u1, const real_T bp0[], const
534 real_T bp1[], const real_T table[], const uint32_T maxIndex[], uint32_T stride)
535{
536 real_T frac;
537 uint32_T bpIndices[2];
538 real_T fractions[2];
539 real_T yL_1d;
540 uint32_T iRght;
541 uint32_T bpIdx;
542 uint32_T iLeft;
543
544 /* Lookup 2-D
545 Search method: 'binary'
546 Use previous index: 'off'
547 Interpolation method: 'Linear'
548 Extrapolation method: 'Linear'
549 Use last breakpoint for index at or above upper limit: 'off'
550 Remove protection against out-of-range input in generated code: 'off'
551 */
552 /* Prelookup - Index and Fraction
553 Index Search method: 'binary'
554 Extrapolation method: 'Linear'
555 Use previous index: 'off'
556 Use last breakpoint for index at or above upper limit: 'off'
557 Remove protection against out-of-range input in generated code: 'off'
558 */
559 if (u0 <= bp0[0U]) {
560 iLeft = 0U;
561 frac = (u0 - bp0[0U]) / (bp0[1U] - bp0[0U]);
562 } else if (u0 < bp0[maxIndex[0U]]) {
563 /* Binary Search */
564 bpIdx = maxIndex[0U] >> 1U;
565 iLeft = 0U;
566 iRght = maxIndex[0U];
567 while (iRght - iLeft > 1U) {
568 if (u0 < bp0[bpIdx]) {
569 iRght = bpIdx;
570 } else {
571 iLeft = bpIdx;
572 }
573
574 bpIdx = (iRght + iLeft) >> 1U;
575 }
576
577 frac = (u0 - bp0[iLeft]) / (bp0[iLeft + 1U] - bp0[iLeft]);
578 } else {
579 iLeft = maxIndex[0U] - 1U;
580 frac = (u0 - bp0[maxIndex[0U] - 1U]) / (bp0[maxIndex[0U]] - bp0[maxIndex[0U]
581 - 1U]);
582 }
583
584 fractions[0U] = frac;
585 bpIndices[0U] = iLeft;
586
587 /* Prelookup - Index and Fraction
588 Index Search method: 'binary'
589 Extrapolation method: 'Linear'
590 Use previous index: 'off'
591 Use last breakpoint for index at or above upper limit: 'off'
592 Remove protection against out-of-range input in generated code: 'off'
593 */
594 if (u1 <= bp1[0U]) {
595 iLeft = 0U;
596 frac = (u1 - bp1[0U]) / (bp1[1U] - bp1[0U]);
597 } else if (u1 < bp1[maxIndex[1U]]) {
598 /* Binary Search */
599 bpIdx = maxIndex[1U] >> 1U;
600 iLeft = 0U;
601 iRght = maxIndex[1U];
602 while (iRght - iLeft > 1U) {
603 if (u1 < bp1[bpIdx]) {
604 iRght = bpIdx;
605 } else {
606 iLeft = bpIdx;
607 }
608
609 bpIdx = (iRght + iLeft) >> 1U;
610 }
611
612 frac = (u1 - bp1[iLeft]) / (bp1[iLeft + 1U] - bp1[iLeft]);
613 } else {
614 iLeft = maxIndex[1U] - 1U;
615 frac = (u1 - bp1[maxIndex[1U] - 1U]) / (bp1[maxIndex[1U]] - bp1[maxIndex[1U]
616 - 1U]);
617 }
618
619 /* Interpolation 2-D
620 Interpolation method: 'Linear'
621 Use last breakpoint for index at or above upper limit: 'off'
622 Overflow mode: 'portable wrapping'
623 */
624 bpIdx = iLeft * stride + bpIndices[0U];
625 yL_1d = (table[bpIdx + 1U] - table[bpIdx]) * fractions[0U] + table[bpIdx];
626 bpIdx += stride;
627 return (((table[bpIdx + 1U] - table[bpIdx]) * fractions[0U] + table[bpIdx]) -
628 yL_1d) * frac + yL_1d;
629}
630
631/* System initialize for atomic system: '<S466>/L Main Gear Geometry Forces and Moments' */
632void AFCS_MODEL1_LMainGearGeometryForcesandMoments_Init(SimStruct * const S,
633 B_LMainGearGeometryForcesandM_T *localB)
634{
635 /* SystemInitialize for Enabled SubSystem: '<S618>/GenerateGroundFM' */
636 /* InitializeConditions for Memory: '<S660>/Xcmpr_prev' */
637 ((real_T *)ssGetDWork(S, 629))[0] = 0.0;
638
639 /* SystemInitialize for Outport: '<S651>/FM' */
640 localB->LocalForcestoBodyAxes1[0] = 0.0;
641 localB->Gain2[0] = 0.0;
642 localB->LocalForcestoBodyAxes1[1] = 0.0;
643 localB->Gain2[1] = 0.0;
644 localB->LocalForcestoBodyAxes1[2] = 0.0;
645 localB->Gain2[2] = 0.0;
646
647 /* SystemInitialize for Outport: '<S651>/GearCmprs' */
648 localB->RangeLimit = 0.0;
649
650 /* End of SystemInitialize for SubSystem: '<S618>/GenerateGroundFM' */
651}
652
653/* Disable for atomic system: '<S466>/L Main Gear Geometry Forces and Moments' */
654void AFCS_MODEL1_LMainGearGeometryForcesandMoments_Disable(SimStruct * const S,
655 B_LMainGearGeometryForcesandM_T *localB)
656{
657 /* Disable for Enabled SubSystem: '<S618>/GenerateGroundFM' */
658 if (((boolean_T *)ssGetDWork(S, 640))[0]) {
659 /* Disable for Outport: '<S651>/FM' */
660 localB->LocalForcestoBodyAxes1[0] = 0.0;
661 localB->Gain2[0] = 0.0;
662 localB->LocalForcestoBodyAxes1[1] = 0.0;
663 localB->Gain2[1] = 0.0;
664 localB->LocalForcestoBodyAxes1[2] = 0.0;
665 localB->Gain2[2] = 0.0;
666
667 /* Disable for Outport: '<S651>/GearCmprs' */
668 localB->RangeLimit = 0.0;
669 ((boolean_T *)ssGetDWork(S, 640))[0] = false;
670 }
671
672 /* End of Disable for SubSystem: '<S618>/GenerateGroundFM' */
673}
674
675/* Start for atomic system: '<S466>/L Main Gear Geometry Forces and Moments' */
676void AFCS_MODEL1_LMainGearGeometryForcesandMoments_Start(SimStruct * const S)
677{
678 /* Start for Enabled SubSystem: '<S618>/GenerateGroundFM' */
679 ((boolean_T *)ssGetDWork(S, 640))[0] = false;
680 ssSetSolverNeedsReset(S);
681}
682
683/* Outputs for atomic system: '<S466>/L Main Gear Geometry Forces and Moments' */
684void AFCS_MODEL1_LMainGearGeometryForcesandMoments(SimStruct * const S, real_T
685 rtu_ground_z_m, real_T rtu_plant, real_T rtu_plant_d, real_T rtu_plant_f,
686 const real_T rtu_GearLoc[3], real_T rtu_FricCoeff, real_T rtu_FricCoeff_g,
687 B_LMainGearGeometryForcesandM_T *localB, ConstB_LMainGearGeometryForce_T
688 *localC)
689{
690 real_T cosOut;
691 int32_T i;
692 if (ssIsSampleHit(S, 1, 0)) {
693 }
694
695 if (1) {
696 /* Sum: '<S618>/Subtract' */
697 localB->Gain = rtu_plant + rtu_ground_z_m;
698
699 /* Switch: '<S618>/Switch1' */
700 localB->Switch1 = (int8_T)!(localB->Gain > 15.0);
701 }
702
703 if (ssIsSampleHit(S, 1, 0)) {
704 /* SignalConversion: '<S618>/HiddenBuf_InsertedFor_GenerateGroundFM_at_inport_5' */
705 localB->HiddenBuf_InsertedFor_GenerateG = localB->Switch1;
706
707 /* Outputs for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:
708 * EnablePort: '<S651>/Enable'
709 */
710 if (ssIsMajorTimeStep(S)) {
711 if (localB->HiddenBuf_InsertedFor_GenerateG > 0) {
712 if (!((boolean_T *)ssGetDWork(S, 640))[0]) {
713 if (ssGetTaskTime(S,1) != ssGetTStart(S)) {
714 ssSetSolverNeedsReset(S);
715 }
716
717 ((boolean_T *)ssGetDWork(S, 640))[0] = true;
718 }
719 } else {
720 if (((boolean_T *)ssGetDWork(S, 640))[0]) {
721 ssSetSolverNeedsReset(S);
722
723 /* Disable for Outport: '<S651>/FM' */
724 localB->LocalForcestoBodyAxes1[0] = 0.0;
725 localB->Gain2[0] = 0.0;
726 localB->LocalForcestoBodyAxes1[1] = 0.0;
727 localB->Gain2[1] = 0.0;
728 localB->LocalForcestoBodyAxes1[2] = 0.0;
729 localB->Gain2[2] = 0.0;
730
731 /* Disable for Outport: '<S651>/GearCmprs' */
732 localB->RangeLimit = 0.0;
733 ((boolean_T *)ssGetDWork(S, 640))[0] = false;
734 }
735 }
736 }
737
738 /* End of Outputs for SubSystem: '<S618>/GenerateGroundFM' */
739 }
740
741 /* Outputs for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:
742 * EnablePort: '<S651>/Enable'
743 */
744 if (((boolean_T *)ssGetDWork(S, 640))[0]) {
745 if (ssIsSampleHit(S, 1, 0)) {
746 /* Memory: '<S660>/Xcmpr_prev' */
747 localB->Xcmpr_prev = ((real_T *)ssGetDWork(S, 629))[0];
748 }
749
750 if (1) {
751 /* SignalConversion: '<S656>/TmpSignal ConversionAtSinCosInport1' incorporates:
752 * Constant: '<S651>/Constant'
753 */
754 localB->SinCos_o1_e[0] = rtu_plant_d;
755 localB->SinCos_o1_e[1] = rtu_plant_f;
756 localB->SinCos_o1_e[2] = 0.0;
757
758 /* Trigonometry: '<S656>/SinCos' */
759 cosOut = cos(localB->SinCos_o1_e[0]);
760 localB->SinCos_o1_e[0] = sin(localB->SinCos_o1_e[0]);
761 localB->TmpSignalConversionAtProduct3In[0] = cosOut;
762 cosOut = cos(localB->SinCos_o1_e[1]);
763 localB->SinCos_o1_e[1] = sin(localB->SinCos_o1_e[1]);
764 localB->TmpSignalConversionAtProduct3In[1] = cosOut;
765 cosOut = cos(localB->SinCos_o1_e[2]);
766 localB->SinCos_o1_e[2] = sin(localB->SinCos_o1_e[2]);
767 localB->TmpSignalConversionAtProduct3In[2] = cosOut;
768
769 /* Product: '<S661>/u(5)*u(6)' */
770 localB->VectorConcatenate_n[0] = localB->TmpSignalConversionAtProduct3In[1]
771 * localB->TmpSignalConversionAtProduct3In[2];
772
773 /* Product: '<S664>/u(6)*u(1)*u(2)' */
774 localB->kxi = localB->TmpSignalConversionAtProduct3In[2] *
775 localB->SinCos_o1_e[0] * localB->SinCos_o1_e[1];
776
777 /* Product: '<S664>/u(3)*u(4)' */
778 localB->compression = localB->SinCos_o1_e[2] *
779 localB->TmpSignalConversionAtProduct3In[0];
780
781 /* Sum: '<S664>/Sum' */
782 localB->VectorConcatenate_n[1] = localB->kxi - localB->compression;
783
784 /* Product: '<S667>/u(1)*u(3)' */
785 localB->compression = localB->SinCos_o1_e[0] * localB->SinCos_o1_e[2];
786
787 /* Product: '<S667>/u(2)*u(4)*u(6)' */
788 localB->kxi = localB->SinCos_o1_e[1] *
789 localB->TmpSignalConversionAtProduct3In[0] *
790 localB->TmpSignalConversionAtProduct3In[2];
791
792 /* Sum: '<S667>/Sum' */
793 localB->VectorConcatenate_n[2] = localB->compression + localB->kxi;
794
795 /* Product: '<S662>/u(3)*u(5)' */
796 localB->VectorConcatenate_n[3] = localB->SinCos_o1_e[2] *
797 localB->TmpSignalConversionAtProduct3In[1];
798
799 /* Product: '<S665>/u(1)*u(2)*u(3)' */
800 localB->compression = localB->SinCos_o1_e[0] * localB->SinCos_o1_e[1] *
801 localB->SinCos_o1_e[2];
802
803 /* Product: '<S665>/u(4)*u(6)' */
804 localB->kxi = localB->TmpSignalConversionAtProduct3In[0] *
805 localB->TmpSignalConversionAtProduct3In[2];
806
807 /* Sum: '<S665>/Sum' */
808 localB->VectorConcatenate_n[4] = localB->compression + localB->kxi;
809
810 /* Product: '<S668>/u(2)*u(3)*u(4)' */
811 localB->compression = localB->SinCos_o1_e[1] * localB->SinCos_o1_e[2] *
812 localB->TmpSignalConversionAtProduct3In[0];
813
814 /* Product: '<S668>/u(1)*u(6)' */
815 localB->kxi = localB->TmpSignalConversionAtProduct3In[2] *
816 localB->SinCos_o1_e[0];
817
818 /* Sum: '<S668>/Sum' */
819 localB->VectorConcatenate_n[5] = localB->compression - localB->kxi;
820
821 /* Gain: '<S663>/Gain2' */
822 localB->VectorConcatenate_n[6] = -localB->SinCos_o1_e[1];
823
824 /* Product: '<S666>/u(1)*u(3)' */
825 localB->VectorConcatenate_n[7] = localB->SinCos_o1_e[0] *
826 localB->TmpSignalConversionAtProduct3In[1];
827
828 /* Product: '<S669>/u(4)*u(5)' */
829 localB->VectorConcatenate_n[8] = localB->TmpSignalConversionAtProduct3In[0]
830 * localB->TmpSignalConversionAtProduct3In[1];
831
832 /* Selector: '<S654>/get_row3' */
833 localB->TmpSignalConversionAtProduct3In[0] = localB->VectorConcatenate_n[2];
834 localB->TmpSignalConversionAtProduct3In[1] = localB->VectorConcatenate_n[5];
835 localB->TmpSignalConversionAtProduct3In[2] = localB->VectorConcatenate_n[8];
836
837 /* Product: '<S654>/prod1' */
838 localB->compression = rtu_GearLoc[0] *
839 localB->TmpSignalConversionAtProduct3In[0];
840
841 /* Product: '<S654>/prod2' */
842 localB->kxi = rtu_GearLoc[1] * localB->TmpSignalConversionAtProduct3In[1];
843
844 /* Sum: '<S654>/Sum1' */
845 localB->Sum1 = ((rtu_ground_z_m + rtu_plant) - localB->compression) -
846 localB->kxi;
847
848 /* Product: '<S660>/compression' */
849 localB->compression = localB->Sum1 /
850 localB->TmpSignalConversionAtProduct3In[2];
851
852 /* Sum: '<S660>/Sum1' */
853 localB->kxi = rtu_GearLoc[2] - localB->compression;
854
855 /* Saturate: '<S660>/RangeLimit' */
856 if (localB->kxi > 0.5) {
857 localB->RangeLimit = 0.5;
858 } else if (localB->kxi < 0.0) {
859 localB->RangeLimit = 0.0;
860 } else {
861 localB->RangeLimit = localB->kxi;
862 }
863
864 /* End of Saturate: '<S660>/RangeLimit' */
865
866 /* Gain: '<S660>/Gain1' */
867 localB->kxi = 10000.0 * localB->RangeLimit;
868
869 /* Sum: '<S660>/dCompression' */
870 localB->jxk = localB->RangeLimit - localB->Xcmpr_prev;
871
872 /* Gain: '<S660>/ComprRate' */
873 localB->jxk *= 100.0;
874
875 /* Gain: '<S660>/Gain' */
876 localB->jxk *= 10000.0;
877
878 /* Sum: '<S660>/Sum2' */
879 localB->kxi += localB->jxk;
880
881 /* Product: '<S660>/NormalForce' */
882 localB->kxi /= localB->TmpSignalConversionAtProduct3In[2];
883
884 /* Saturate: '<S660>/ground only pushes' */
885 if (localB->kxi <= 0.0) {
886 localB->kxi = 0.0;
887 }
888
889 /* End of Saturate: '<S660>/ground only pushes' */
890
891 /* Gain: '<S660>/reaction' */
892 localB->reaction = -localB->kxi;
893
894 /* Product: '<S651>/Product8' */
895 localB->GearLocalForcevector[0] = localB->reaction * rtu_FricCoeff;
896
897 /* Product: '<S651>/Product9' */
898 localB->GearLocalForcevector[1] = localB->reaction * rtu_FricCoeff_g;
899
900 /* SignalConversion: '<S651>/ConcatBufferAtGear Local Force vectorIn2' */
901 localB->GearLocalForcevector[2] = localB->reaction;
902
903 /* Product: '<S651>/Local Forces to Body Axes1' */
904 for (i = 0; i < 3; i++) {
905 localB->LocalForcestoBodyAxes1[i] = 0.0;
906 localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_n[i] *
907 localB->GearLocalForcevector[0];
908 localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_n[i + 3] *
909 localB->GearLocalForcevector[1];
910 localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_n[i + 6] *
911 localB->GearLocalForcevector[2];
912 }
913
914 /* End of Product: '<S651>/Local Forces to Body Axes1' */
915
916 /* Product: '<S658>/j x k' */
917 localB->jxk = rtu_GearLoc[1] * localB->LocalForcestoBodyAxes1[2];
918
919 /* Product: '<S658>/k x i' */
920 localB->kxi = rtu_GearLoc[2] * localB->LocalForcestoBodyAxes1[0];
921
922 /* Product: '<S658>/i x j' */
923 localB->ixj = rtu_GearLoc[0] * localB->LocalForcestoBodyAxes1[1];
924
925 /* Product: '<S659>/k x j' */
926 localB->kxj = rtu_GearLoc[2] * localB->LocalForcestoBodyAxes1[1];
927
928 /* Product: '<S659>/i x k' */
929 localB->ixk = rtu_GearLoc[0] * localB->LocalForcestoBodyAxes1[2];
930
931 /* Product: '<S659>/j x i' */
932 localB->jxi = rtu_GearLoc[1] * localB->LocalForcestoBodyAxes1[0];
933
934 /* Sum: '<S653>/Sum' */
935 localB->Sum[0] = localB->jxk - localB->kxj;
936 localB->Sum[1] = localB->kxi - localB->ixk;
937 localB->Sum[2] = localB->ixj - localB->jxi;
938 }
939
940 if (ssIsSampleHit(S, 1, 0)) {
941 /* Constant: '<S657>/zero1' */
942 localB->VectorConcatenate_b[0] = 0.0;
943 }
944
945 if (1) {
946 /* SignalConversion: '<S671>/ConcatBufferAtVector ConcatenateIn2' */
947 localB->VectorConcatenate_b[1] = localB->compression;
948
949 /* SignalConversion: '<S671>/ConcatBufferAtVector ConcatenateIn3' */
950 localB->VectorConcatenate_b[2] = localC->inv3;
951
952 /* Gain: '<S657>/inv1' */
953 localB->VectorConcatenate_b[3] = -localB->compression;
954 }
955
956 if (ssIsSampleHit(S, 1, 0)) {
957 /* Constant: '<S657>/zero2' */
958 localB->VectorConcatenate_b[4] = 0.0;
959 }
960
961 if (1) {
962 /* SignalConversion: '<S671>/ConcatBufferAtVector ConcatenateIn6' */
963 localB->VectorConcatenate_b[5] = rtu_GearLoc[0];
964
965 /* SignalConversion: '<S671>/ConcatBufferAtVector ConcatenateIn7' */
966 localB->VectorConcatenate_b[6] = rtu_GearLoc[1];
967
968 /* SignalConversion: '<S671>/ConcatBufferAtVector ConcatenateIn8' */
969 localB->VectorConcatenate_b[7] = localC->inv2;
970 }
971
972 if (ssIsSampleHit(S, 1, 0)) {
973 /* Constant: '<S657>/zero3' */
974 localB->VectorConcatenate_b[8] = 0.0;
975 }
976
977 if (1) {
978 for (i = 0; i < 3; i++) {
979 /* Product: '<S655>/force to moment' */
980 localB->TmpSignalConversionAtProduct3In[i] = 0.0;
981 localB->TmpSignalConversionAtProduct3In[i] +=
982 localB->VectorConcatenate_b[3 * i] * localB->LocalForcestoBodyAxes1[0];
983 localB->TmpSignalConversionAtProduct3In[i] +=
984 localB->VectorConcatenate_b[3 * i + 1] *
985 localB->LocalForcestoBodyAxes1[1];
986 localB->TmpSignalConversionAtProduct3In[i] +=
987 localB->VectorConcatenate_b[3 * i + 2] *
988 localB->LocalForcestoBodyAxes1[2];
989
990 /* Gain: '<S651>/Gain2' */
991 localB->Gain2[i] = AFCS_MODEL1_ConstP.pooled20[i] *
992 localB->TmpSignalConversionAtProduct3In[i];
993 }
994 }
995
996 if (ssIsSampleHit(S, 1, 0)) {
997 }
998
999 if (1) {
1000 /* Math: '<S654>/Math Function1' */
1001 for (i = 0; i < 3; i++) {
1002 localB->MathFunction1[3 * i] = localB->VectorConcatenate_n[i];
1003 localB->MathFunction1[1 + 3 * i] = localB->VectorConcatenate_n[i + 3];
1004 localB->MathFunction1[2 + 3 * i] = localB->VectorConcatenate_n[i + 6];
1005 }
1006
1007 /* End of Math: '<S654>/Math Function1' */
1008
1009 /* SignalConversion: '<S654>/TmpSignal ConversionAtProduct3Inport2' incorporates:
1010 * Constant: '<S654>/Constant1'
1011 */
1012 localB->TmpSignalConversionAtProduct3In[0] = rtu_GearLoc[0];
1013 localB->TmpSignalConversionAtProduct3In[1] = rtu_GearLoc[1];
1014 localB->TmpSignalConversionAtProduct3In[2] = 0.0;
1015
1016 /* Product: '<S654>/Product3' */
1017 for (i = 0; i < 3; i++) {
1018 localB->Product3[i] = 0.0;
1019 localB->Product3[i] += localB->MathFunction1[i] *
1020 localB->TmpSignalConversionAtProduct3In[0];
1021 localB->Product3[i] += localB->MathFunction1[i + 3] *
1022 localB->TmpSignalConversionAtProduct3In[1];
1023 localB->Product3[i] += localB->MathFunction1[i + 6] *
1024 localB->TmpSignalConversionAtProduct3In[2];
1025 }
1026
1027 /* End of Product: '<S654>/Product3' */
1028
1029 /* Sum: '<S654>/Sum5' */
1030 localB->Sum5 = rtu_plant - localB->Product3[2];
1031 }
1032
1033 if (ssIsSampleHit(S, 1, 0)) {
1034 }
1035 }
1036
1037 /* End of Outputs for SubSystem: '<S618>/GenerateGroundFM' */
1038 if (ssIsSampleHit(S, 1, 0)) {
1039 }
1040
1041 if (1) {
1042 /* Gain: '<S618>/Gain' */
1043 localB->Gain = -localB->LocalForcestoBodyAxes1[2];
1044
1045 /* SignalConversion: '<S652>/TmpSignal ConversionAtSinCosInport1' incorporates:
1046 * Constant: '<S618>/Constant'
1047 */
1048 localB->SinCos_o1[0] = rtu_plant_d;
1049 localB->SinCos_o1[1] = rtu_plant_f;
1050 localB->SinCos_o1[2] = 0.0;
1051
1052 /* Trigonometry: '<S652>/SinCos' */
1053 cosOut = cos(localB->SinCos_o1[0]);
1054 localB->SinCos_o1[0] = sin(localB->SinCos_o1[0]);
1055 localB->SinCos_o2[0] = cosOut;
1056 cosOut = cos(localB->SinCos_o1[1]);
1057 localB->SinCos_o1[1] = sin(localB->SinCos_o1[1]);
1058 localB->SinCos_o2[1] = cosOut;
1059 cosOut = cos(localB->SinCos_o1[2]);
1060 localB->SinCos_o1[2] = sin(localB->SinCos_o1[2]);
1061 localB->SinCos_o2[2] = cosOut;
1062
1063 /* Product: '<S672>/u(5)*u(6)' */
1064 localB->VectorConcatenate[0] = localB->SinCos_o2[1] * localB->SinCos_o2[2];
1065
1066 /* Product: '<S673>/u(3)*u(5)' */
1067 localB->VectorConcatenate[3] = localB->SinCos_o1[2] * localB->SinCos_o2[1];
1068
1069 /* Gain: '<S674>/Gain2' */
1070 localB->VectorConcatenate[6] = -localB->SinCos_o1[1];
1071
1072 /* Product: '<S675>/u(6)*u(1)*u(2)' */
1073 localB->u1u6 = localB->SinCos_o2[2] * localB->SinCos_o1[0] *
1074 localB->SinCos_o1[1];
1075
1076 /* Product: '<S675>/u(3)*u(4)' */
1077 localB->u2u3u4 = localB->SinCos_o1[2] * localB->SinCos_o2[0];
1078
1079 /* Sum: '<S675>/Sum' */
1080 localB->VectorConcatenate[1] = localB->u1u6 - localB->u2u3u4;
1081
1082 /* Product: '<S676>/u(1)*u(2)*u(3)' */
1083 localB->u2u3u4 = localB->SinCos_o1[0] * localB->SinCos_o1[1] *
1084 localB->SinCos_o1[2];
1085
1086 /* Product: '<S676>/u(4)*u(6)' */
1087 localB->u1u6 = localB->SinCos_o2[0] * localB->SinCos_o2[2];
1088
1089 /* Sum: '<S676>/Sum' */
1090 localB->VectorConcatenate[4] = localB->u2u3u4 + localB->u1u6;
1091
1092 /* Product: '<S677>/u(1)*u(3)' */
1093 localB->VectorConcatenate[7] = localB->SinCos_o1[0] * localB->SinCos_o2[1];
1094
1095 /* Product: '<S678>/u(1)*u(3)' */
1096 localB->u2u3u4 = localB->SinCos_o1[0] * localB->SinCos_o1[2];
1097
1098 /* Product: '<S678>/u(2)*u(4)*u(6)' */
1099 localB->u1u6 = localB->SinCos_o1[1] * localB->SinCos_o2[0] *
1100 localB->SinCos_o2[2];
1101
1102 /* Sum: '<S678>/Sum' */
1103 localB->VectorConcatenate[2] = localB->u2u3u4 + localB->u1u6;
1104
1105 /* Product: '<S679>/u(2)*u(3)*u(4)' */
1106 localB->u2u3u4 = localB->SinCos_o1[1] * localB->SinCos_o1[2] *
1107 localB->SinCos_o2[0];
1108
1109 /* Product: '<S679>/u(1)*u(6)' */
1110 localB->u1u6 = localB->SinCos_o2[2] * localB->SinCos_o1[0];
1111
1112 /* Sum: '<S679>/Sum' */
1113 localB->VectorConcatenate[5] = localB->u2u3u4 - localB->u1u6;
1114
1115 /* Product: '<S680>/u(4)*u(5)' */
1116 localB->VectorConcatenate[8] = localB->SinCos_o2[0] * localB->SinCos_o2[1];
1117
1118 /* Switch: '<S618>/Switch' */
1119 localB->Switch = (int8_T)(localB->Gain > 3.0);
1120 }
1121}
1122
1123/* Update for atomic system: '<S466>/L Main Gear Geometry Forces and Moments' */
1124void AFCS_MODEL1_LMainGearGeometryForcesandMoments_Update(SimStruct * const S,
1125 B_LMainGearGeometryForcesandM_T *localB)
1126{
1127 /* Update for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:
1128 * Update for EnablePort: '<S651>/Enable'
1129 */
1130 if (((boolean_T *)ssGetDWork(S, 640))[0] && ssIsSampleHit(S, 1, 0)) {
1131 /* Update for Memory: '<S660>/Xcmpr_prev' */
1132 ((real_T *)ssGetDWork(S, 629))[0] = localB->RangeLimit;
1133 }
1134
1135 /* End of Update for SubSystem: '<S618>/GenerateGroundFM' */
1136}
1137
1138/* Termination for atomic system: '<S466>/L Main Gear Geometry Forces and Moments' */
1139void AFCS_MODEL1_LMainGearGeometryForcesandMoments_Term(SimStruct *const S)
1140{
1141}
1142
1143/* System initialize for atomic system: '<S466>/Nose Gear Geometry Forces and Moments' */
1144void AFCS_MODEL1_NoseGearGeometryForcesandMoments_Init(SimStruct * const S,
1145 B_NoseGearGeometryForcesandMo_T *localB)
1146{
1147 /* SystemInitialize for Enabled SubSystem: '<S620>/GenerateGroundFM' */
1148 /* InitializeConditions for Memory: '<S693>/Xcmpr_prev' */
1149 ((real_T *)ssGetDWork(S, 624))[0] = 0.0;
1150
1151 /* SystemInitialize for Outport: '<S686>/FM' */
1152 localB->LocalForcestoBodyAxes1[0] = 0.0;
1153 localB->Gain1[0] = 0.0;
1154 localB->LocalForcestoBodyAxes1[1] = 0.0;
1155 localB->Gain1[1] = 0.0;
1156 localB->LocalForcestoBodyAxes1[2] = 0.0;
1157 localB->Gain1[2] = 0.0;
1158
1159 /* SystemInitialize for Outport: '<S686>/GearCmprs' */
1160 localB->RangeLimit = 0.0;
1161
1162 /* End of SystemInitialize for SubSystem: '<S620>/GenerateGroundFM' */
1163}
1164
1165/* Disable for atomic system: '<S466>/Nose Gear Geometry Forces and Moments' */
1166void AFCS_MODEL1_NoseGearGeometryForcesandMoments_Disable(SimStruct * const S,
1167 B_NoseGearGeometryForcesandMo_T *localB)
1168{
1169 /* Disable for Enabled SubSystem: '<S620>/GenerateGroundFM' */
1170 if (((boolean_T *)ssGetDWork(S, 628))[0]) {
1171 /* Disable for Outport: '<S686>/FM' */
1172 localB->LocalForcestoBodyAxes1[0] = 0.0;
1173 localB->Gain1[0] = 0.0;
1174 localB->LocalForcestoBodyAxes1[1] = 0.0;
1175 localB->Gain1[1] = 0.0;
1176 localB->LocalForcestoBodyAxes1[2] = 0.0;
1177 localB->Gain1[2] = 0.0;
1178
1179 /* Disable for Outport: '<S686>/GearCmprs' */
1180 localB->RangeLimit = 0.0;
1181 ((boolean_T *)ssGetDWork(S, 628))[0] = false;
1182 }
1183
1184 /* End of Disable for SubSystem: '<S620>/GenerateGroundFM' */
1185}
1186
1187/* Start for atomic system: '<S466>/Nose Gear Geometry Forces and Moments' */
1188void AFCS_MODEL1_NoseGearGeometryForcesandMoments_Start(SimStruct * const S)
1189{
1190 /* Start for Enabled SubSystem: '<S620>/GenerateGroundFM' */
1191 ((boolean_T *)ssGetDWork(S, 628))[0] = false;
1192 ssSetSolverNeedsReset(S);
1193}
1194
1195/* Outputs for atomic system: '<S466>/Nose Gear Geometry Forces and Moments' */
1196void AFCS_MODEL1_NoseGearGeometryForcesandMoments(SimStruct * const S, real_T
1197 rtu_ground_z_m, real_T rtu_plant, real_T rtu_plant_a, real_T rtu_plant_j,
1198 const real_T rtu_GearLoc[3], real_T rtu_FricCoeff, real_T rtu_FricCoeff_j,
1199 const real_T rtu_Euler[3], B_NoseGearGeometryForcesandMo_T *localB,
1200 ConstB_NoseGearGeometryForces_T *localC)
1201{
1202 real_T cosOut;
1203 int32_T i;
1204 if (1) {
1205 /* Sum: '<S620>/Subtract' */
1206 localB->Gain = rtu_plant + rtu_ground_z_m;
1207
1208 /* Switch: '<S620>/Switch1' */
1209 localB->Switch1 = (int8_T)!(localB->Gain > 15.0);
1210 }
1211
1212 if (ssIsSampleHit(S, 1, 0)) {
1213 /* SignalConversion: '<S620>/HiddenBuf_InsertedFor_GenerateGroundFM_at_inport_5' */
1214 localB->HiddenBuf_InsertedFor_GenerateG = localB->Switch1;
1215
1216 /* Outputs for Enabled SubSystem: '<S620>/GenerateGroundFM' incorporates:
1217 * EnablePort: '<S686>/Enable'
1218 */
1219 if (ssIsMajorTimeStep(S)) {
1220 if (localB->HiddenBuf_InsertedFor_GenerateG > 0) {
1221 if (!((boolean_T *)ssGetDWork(S, 628))[0]) {
1222 /* Outputs for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:
1223 * EnablePort: '<S651>/Enable'
1224 */
1225 if (ssGetTaskTime(S,1) != ssGetTStart(S)) {
1226 ssSetSolverNeedsReset(S);
1227 }
1228
1229 /* End of Outputs for SubSystem: '<S618>/GenerateGroundFM' */
1230 ((boolean_T *)ssGetDWork(S, 628))[0] = true;
1231 }
1232 } else {
1233 if (((boolean_T *)ssGetDWork(S, 628))[0]) {
1234 ssSetSolverNeedsReset(S);
1235
1236 /* Disable for Outport: '<S686>/FM' */
1237 localB->LocalForcestoBodyAxes1[0] = 0.0;
1238 localB->Gain1[0] = 0.0;
1239 localB->LocalForcestoBodyAxes1[1] = 0.0;
1240 localB->Gain1[1] = 0.0;
1241 localB->LocalForcestoBodyAxes1[2] = 0.0;
1242 localB->Gain1[2] = 0.0;
1243
1244 /* Disable for Outport: '<S686>/GearCmprs' */
1245 localB->RangeLimit = 0.0;
1246 ((boolean_T *)ssGetDWork(S, 628))[0] = false;
1247 }
1248 }
1249 }
1250
1251 /* End of Outputs for SubSystem: '<S620>/GenerateGroundFM' */
1252 }
1253
1254 /* Outputs for Enabled SubSystem: '<S620>/GenerateGroundFM' incorporates:
1255 * EnablePort: '<S686>/Enable'
1256 */
1257 if (((boolean_T *)ssGetDWork(S, 628))[0]) {
1258 if (ssIsSampleHit(S, 1, 0)) {
1259 /* Memory: '<S693>/Xcmpr_prev' */
1260 localB->Xcmpr_prev = ((real_T *)ssGetDWork(S, 624))[0];
1261 }
1262
1263 if (1) {
1264 /* SignalConversion: '<S690>/TmpSignal ConversionAtSinCosInport1' incorporates:
1265 * Constant: '<S686>/Constant'
1266 */
1267 localB->Product3[0] = rtu_plant_a;
1268 localB->Product3[1] = rtu_plant_j;
1269 localB->Product3[2] = 0.0;
1270
1271 /* Trigonometry: '<S690>/SinCos' */
1272 cosOut = cos(localB->Product3[0]);
1273 localB->Product3[0] = sin(localB->Product3[0]);
1274 localB->sincos_o2[0] = cosOut;
1275 cosOut = cos(localB->Product3[1]);
1276 localB->Product3[1] = sin(localB->Product3[1]);
1277 localB->sincos_o2[1] = cosOut;
1278 cosOut = cos(localB->Product3[2]);
1279 localB->Product3[2] = sin(localB->Product3[2]);
1280 localB->sincos_o2[2] = cosOut;
1281
1282 /* Product: '<S694>/u(5)*u(6)' */
1283 localB->VectorConcatenate_c[0] = localB->sincos_o2[1] * localB->sincos_o2
1284 [2];
1285
1286 /* Product: '<S697>/u(6)*u(1)*u(2)' */
1287 localB->prod2 = localB->sincos_o2[2] * localB->Product3[0] *
1288 localB->Product3[1];
1289
1290 /* Product: '<S697>/u(3)*u(4)' */
1291 localB->compression = localB->Product3[2] * localB->sincos_o2[0];
1292
1293 /* Sum: '<S697>/Sum' */
1294 localB->VectorConcatenate_c[1] = localB->prod2 - localB->compression;
1295
1296 /* Product: '<S700>/u(1)*u(3)' */
1297 localB->compression = localB->Product3[0] * localB->Product3[2];
1298
1299 /* Product: '<S700>/u(2)*u(4)*u(6)' */
1300 localB->prod2 = localB->Product3[1] * localB->sincos_o2[0] *
1301 localB->sincos_o2[2];
1302
1303 /* Sum: '<S700>/Sum' */
1304 localB->VectorConcatenate_c[2] = localB->compression + localB->prod2;
1305
1306 /* Product: '<S695>/u(3)*u(5)' */
1307 localB->VectorConcatenate_c[3] = localB->Product3[2] * localB->sincos_o2[1];
1308
1309 /* Product: '<S698>/u(1)*u(2)*u(3)' */
1310 localB->compression = localB->Product3[0] * localB->Product3[1] *
1311 localB->Product3[2];
1312
1313 /* Product: '<S698>/u(4)*u(6)' */
1314 localB->prod2 = localB->sincos_o2[0] * localB->sincos_o2[2];
1315
1316 /* Sum: '<S698>/Sum' */
1317 localB->VectorConcatenate_c[4] = localB->compression + localB->prod2;
1318
1319 /* Product: '<S701>/u(2)*u(3)*u(4)' */
1320 localB->compression = localB->Product3[1] * localB->Product3[2] *
1321 localB->sincos_o2[0];
1322
1323 /* Product: '<S701>/u(1)*u(6)' */
1324 localB->prod2 = localB->sincos_o2[2] * localB->Product3[0];
1325
1326 /* Sum: '<S701>/Sum' */
1327 localB->VectorConcatenate_c[5] = localB->compression - localB->prod2;
1328
1329 /* Gain: '<S696>/Gain2' */
1330 localB->VectorConcatenate_c[6] = -localB->Product3[1];
1331
1332 /* Product: '<S699>/u(1)*u(3)' */
1333 localB->VectorConcatenate_c[7] = localB->Product3[0] * localB->sincos_o2[1];
1334
1335 /* Product: '<S702>/u(4)*u(5)' */
1336 localB->VectorConcatenate_c[8] = localB->sincos_o2[0] * localB->sincos_o2
1337 [1];
1338
1339 /* Math: '<S688>/Math Function1' */
1340 for (i = 0; i < 3; i++) {
1341 localB->MathFunction1[3 * i] = localB->VectorConcatenate_c[i];
1342 localB->MathFunction1[1 + 3 * i] = localB->VectorConcatenate_c[i + 3];
1343 localB->MathFunction1[2 + 3 * i] = localB->VectorConcatenate_c[i + 6];
1344 }
1345
1346 /* End of Math: '<S688>/Math Function1' */
1347
1348 /* SignalConversion: '<S688>/TmpSignal ConversionAtProduct3Inport2' incorporates:
1349 * Constant: '<S688>/Constant1'
1350 */
1351 localB->sincos_o2[0] = rtu_GearLoc[0];
1352 localB->sincos_o2[1] = rtu_GearLoc[1];
1353 localB->sincos_o2[2] = 0.0;
1354
1355 /* Product: '<S688>/Product3' */
1356 for (i = 0; i < 3; i++) {
1357 localB->Product3[i] = 0.0;
1358 localB->Product3[i] += localB->MathFunction1[i] * localB->sincos_o2[0];
1359 localB->Product3[i] += localB->MathFunction1[i + 3] * localB->sincos_o2
1360 [1];
1361 localB->Product3[i] += localB->MathFunction1[i + 6] * localB->sincos_o2
1362 [2];
1363 }
1364
1365 /* End of Product: '<S688>/Product3' */
1366
1367 /* Sum: '<S688>/Sum5' */
1368 localB->compression = rtu_plant - localB->Product3[2];
1369
1370 /* Sum: '<S688>/Sum2' */
1371 localB->compression += rtu_ground_z_m;
1372
1373 /* Selector: '<S688>/get_row3' */
1374 localB->sincos_o2[0] = localB->VectorConcatenate_c[2];
1375 localB->sincos_o2[1] = localB->VectorConcatenate_c[5];
1376 localB->sincos_o2[2] = localB->VectorConcatenate_c[8];
1377
1378 /* Product: '<S693>/compression' */
1379 localB->compression /= localB->sincos_o2[2];
1380
1381 /* Sum: '<S693>/Sum1' */
1382 localB->prod2 = rtu_GearLoc[2] - localB->compression;
1383
1384 /* Saturate: '<S693>/RangeLimit' */
1385 if (localB->prod2 > 0.5) {
1386 localB->RangeLimit = 0.5;
1387 } else if (localB->prod2 < 0.0) {
1388 localB->RangeLimit = 0.0;
1389 } else {
1390 localB->RangeLimit = localB->prod2;
1391 }
1392
1393 /* End of Saturate: '<S693>/RangeLimit' */
1394
1395 /* Gain: '<S693>/Gain1' */
1396 localB->prod2 = 10000.0 * localB->RangeLimit;
1397
1398 /* Sum: '<S693>/dCompression' */
1399 localB->prod1 = localB->RangeLimit - localB->Xcmpr_prev;
1400
1401 /* Gain: '<S693>/ComprRate' */
1402 localB->prod1 *= 100.0;
1403
1404 /* Gain: '<S693>/Gain2' */
1405 localB->prod1 *= 10000.0;
1406
1407 /* Sum: '<S693>/Sum2' */
1408 localB->prod2 += localB->prod1;
1409
1410 /* Product: '<S693>/NormalForce' */
1411 localB->prod2 /= localB->sincos_o2[2];
1412
1413 /* Saturate: '<S693>/ground only pushes' */
1414 if (localB->prod2 <= 0.0) {
1415 localB->prod2 = 0.0;
1416 }
1417
1418 /* End of Saturate: '<S693>/ground only pushes' */
1419
1420 /* Gain: '<S693>/reaction' */
1421 localB->reaction = -localB->prod2;
1422 }
1423
1424 if (ssIsSampleHit(S, 1, 0)) {
1425 }
1426
1427 if (1) {
1428 /* Product: '<S686>/Product8' */
1429 localB->sincos_o1[0] = localB->reaction * rtu_FricCoeff;
1430
1431 /* Product: '<S686>/Product9' */
1432 localB->sincos_o1[1] = localB->reaction * rtu_FricCoeff_j;
1433
1434 /* Gain: '<S686>/Gain2' */
1435 localB->sincos_o1[2] = localB->reaction;
1436
1437 /* Product: '<S686>/Local Forces to Body Axes1' */
1438 for (i = 0; i < 3; i++) {
1439 localB->LocalForcestoBodyAxes1[i] = 0.0;
1440 localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_c[i] *
1441 localB->sincos_o1[0];
1442 localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_c[i + 3] *
1443 localB->sincos_o1[1];
1444 localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_c[i + 6] *
1445 localB->sincos_o1[2];
1446 }
1447
1448 /* End of Product: '<S686>/Local Forces to Body Axes1' */
1449 }
1450
1451 if (ssIsSampleHit(S, 1, 0)) {
1452 }
1453
1454 if (1) {
1455 /* Product: '<S688>/prod1' */
1456 localB->prod1 = rtu_GearLoc[0] * localB->sincos_o2[0];
1457
1458 /* Product: '<S688>/prod2' */
1459 localB->prod2 = rtu_GearLoc[1] * localB->sincos_o2[1];
1460
1461 /* Sum: '<S688>/Sum1' */
1462 localB->Sum1 = ((rtu_ground_z_m + rtu_plant) - localB->prod1) -
1463 localB->prod2;
1464 }
1465
1466 if (ssIsSampleHit(S, 1, 0)) {
1467 /* Constant: '<S691>/zero1' */
1468 localB->VectorConcatenate_p[0] = 0.0;
1469 }
1470
1471 if (1) {
1472 /* SignalConversion: '<S704>/ConcatBufferAtVector ConcatenateIn2' */
1473 localB->VectorConcatenate_p[1] = localB->compression;
1474
1475 /* SignalConversion: '<S704>/ConcatBufferAtVector ConcatenateIn3' */
1476 localB->VectorConcatenate_p[2] = localC->inv3;
1477
1478 /* Gain: '<S691>/inv1' */
1479 localB->VectorConcatenate_p[3] = -localB->compression;
1480 }
1481
1482 if (ssIsSampleHit(S, 1, 0)) {
1483 /* Constant: '<S691>/zero2' */
1484 localB->VectorConcatenate_p[4] = 0.0;
1485 }
1486
1487 if (1) {
1488 /* SignalConversion: '<S704>/ConcatBufferAtVector ConcatenateIn6' */
1489 localB->VectorConcatenate_p[5] = rtu_GearLoc[0];
1490
1491 /* SignalConversion: '<S704>/ConcatBufferAtVector ConcatenateIn7' */
1492 localB->VectorConcatenate_p[6] = rtu_GearLoc[1];
1493
1494 /* SignalConversion: '<S704>/ConcatBufferAtVector ConcatenateIn8' */
1495 localB->VectorConcatenate_p[7] = localC->inv2;
1496 }
1497
1498 if (ssIsSampleHit(S, 1, 0)) {
1499 /* Constant: '<S691>/zero3' */
1500 localB->VectorConcatenate_p[8] = 0.0;
1501 }
1502
1503 if (1) {
1504 /* Product: '<S689>/force to moment' */
1505 for (i = 0; i < 3; i++) {
1506 localB->sincos_o1[i] = 0.0;
1507 localB->sincos_o1[i] += localB->VectorConcatenate_p[3 * i] *
1508 localB->LocalForcestoBodyAxes1[0];
1509 localB->sincos_o1[i] += localB->VectorConcatenate_p[3 * i + 1] *
1510 localB->LocalForcestoBodyAxes1[1];
1511 localB->sincos_o1[i] += localB->VectorConcatenate_p[3 * i + 2] *
1512 localB->LocalForcestoBodyAxes1[2];
1513 }
1514
1515 /* End of Product: '<S689>/force to moment' */
1516
1517 /* Gain: '<S686>/Gain1' */
1518 for (i = 0; i < 3; i++) {
1519 localB->Gain1[i] = 0.0;
1520 localB->Gain1[i] += AFCS_MODEL1_ConstP.Gain1_Gain[i] * localB->
1521 sincos_o1[0];
1522 localB->Gain1[i] += AFCS_MODEL1_ConstP.Gain1_Gain[i + 3] *
1523 localB->sincos_o1[1];
1524 localB->Gain1[i] += AFCS_MODEL1_ConstP.Gain1_Gain[i + 6] *
1525 localB->sincos_o1[2];
1526 }
1527
1528 /* End of Gain: '<S686>/Gain1' */
1529
1530 /* Trigonometry: '<S705>/sincos' */
1531 localB->sincos_o1[0] = sin(rtu_Euler[0]);
1532 localB->sincos_o2[0] = cos(rtu_Euler[0]);
1533 localB->sincos_o1[1] = sin(rtu_Euler[1]);
1534 localB->sincos_o2[1] = cos(rtu_Euler[1]);
1535 localB->sincos_o1[2] = sin(rtu_Euler[2]);
1536 localB->sincos_o2[2] = cos(rtu_Euler[2]);
1537
1538 /* Fcn: '<S705>/Fcn11' */
1539 localB->VectorConcatenate_p2[0] = localB->sincos_o2[1] * localB->
1540 sincos_o2[0];
1541
1542 /* Fcn: '<S705>/Fcn21' */
1543 localB->VectorConcatenate_p2[1] = localB->sincos_o1[2] * localB->
1544 sincos_o1[1] * localB->sincos_o2[0] - localB->sincos_o2[2] *
1545 localB->sincos_o1[0];
1546
1547 /* Fcn: '<S705>/Fcn31' */
1548 localB->VectorConcatenate_p2[2] = localB->sincos_o2[2] * localB->
1549 sincos_o1[1] * localB->sincos_o2[0] + localB->sincos_o1[2] *
1550 localB->sincos_o1[0];
1551
1552 /* Fcn: '<S705>/Fcn12' */
1553 localB->VectorConcatenate_p2[3] = localB->sincos_o2[1] * localB->
1554 sincos_o1[0];
1555
1556 /* Fcn: '<S705>/Fcn22' */
1557 localB->VectorConcatenate_p2[4] = localB->sincos_o1[2] * localB->
1558 sincos_o1[1] * localB->sincos_o1[0] + localB->sincos_o2[2] *
1559 localB->sincos_o2[0];
1560
1561 /* Fcn: '<S705>/Fcn32' */
1562 localB->VectorConcatenate_p2[5] = localB->sincos_o2[2] * localB->
1563 sincos_o1[1] * localB->sincos_o1[0] - localB->sincos_o1[2] *
1564 localB->sincos_o2[0];
1565
1566 /* Fcn: '<S705>/Fcn13' */
1567 localB->VectorConcatenate_p2[6] = -localB->sincos_o1[1];
1568
1569 /* Fcn: '<S705>/Fcn23' */
1570 localB->VectorConcatenate_p2[7] = localB->sincos_o1[2] * localB->
1571 sincos_o2[1];
1572
1573 /* Fcn: '<S705>/Fcn33' */
1574 localB->VectorConcatenate_p2[8] = localB->sincos_o2[2] * localB->
1575 sincos_o2[1];
1576 }
1577 }
1578
1579 /* End of Outputs for SubSystem: '<S620>/GenerateGroundFM' */
1580 if (1) {
1581 /* Gain: '<S620>/Gain' */
1582 localB->Gain = -localB->LocalForcestoBodyAxes1[2];
1583
1584 /* SignalConversion: '<S687>/TmpSignal ConversionAtSinCosInport1' incorporates:
1585 * Constant: '<S620>/Constant'
1586 */
1587 localB->SinCos_o1[0] = rtu_plant_a;
1588 localB->SinCos_o1[1] = rtu_plant_j;
1589 localB->SinCos_o1[2] = 0.0;
1590
1591 /* Trigonometry: '<S687>/SinCos' */
1592 cosOut = cos(localB->SinCos_o1[0]);
1593 localB->SinCos_o1[0] = sin(localB->SinCos_o1[0]);
1594 localB->SinCos_o2[0] = cosOut;
1595 cosOut = cos(localB->SinCos_o1[1]);
1596 localB->SinCos_o1[1] = sin(localB->SinCos_o1[1]);
1597 localB->SinCos_o2[1] = cosOut;
1598 cosOut = cos(localB->SinCos_o1[2]);
1599 localB->SinCos_o1[2] = sin(localB->SinCos_o1[2]);
1600 localB->SinCos_o2[2] = cosOut;
1601
1602 /* Product: '<S707>/u(5)*u(6)' */
1603 localB->VectorConcatenate[0] = localB->SinCos_o2[1] * localB->SinCos_o2[2];
1604
1605 /* Product: '<S708>/u(3)*u(5)' */
1606 localB->VectorConcatenate[3] = localB->SinCos_o1[2] * localB->SinCos_o2[1];
1607
1608 /* Gain: '<S709>/Gain2' */
1609 localB->VectorConcatenate[6] = -localB->SinCos_o1[1];
1610
1611 /* Product: '<S710>/u(6)*u(1)*u(2)' */
1612 localB->u1u6 = localB->SinCos_o2[2] * localB->SinCos_o1[0] *
1613 localB->SinCos_o1[1];
1614
1615 /* Product: '<S710>/u(3)*u(4)' */
1616 localB->u2u3u4 = localB->SinCos_o1[2] * localB->SinCos_o2[0];
1617
1618 /* Sum: '<S710>/Sum' */
1619 localB->VectorConcatenate[1] = localB->u1u6 - localB->u2u3u4;
1620
1621 /* Product: '<S711>/u(1)*u(2)*u(3)' */
1622 localB->u2u3u4 = localB->SinCos_o1[0] * localB->SinCos_o1[1] *
1623 localB->SinCos_o1[2];
1624
1625 /* Product: '<S711>/u(4)*u(6)' */
1626 localB->u1u6 = localB->SinCos_o2[0] * localB->SinCos_o2[2];
1627
1628 /* Sum: '<S711>/Sum' */
1629 localB->VectorConcatenate[4] = localB->u2u3u4 + localB->u1u6;
1630
1631 /* Product: '<S712>/u(1)*u(3)' */
1632 localB->VectorConcatenate[7] = localB->SinCos_o1[0] * localB->SinCos_o2[1];
1633
1634 /* Product: '<S713>/u(1)*u(3)' */
1635 localB->u2u3u4 = localB->SinCos_o1[0] * localB->SinCos_o1[2];
1636
1637 /* Product: '<S713>/u(2)*u(4)*u(6)' */
1638 localB->u1u6 = localB->SinCos_o1[1] * localB->SinCos_o2[0] *
1639 localB->SinCos_o2[2];
1640
1641 /* Sum: '<S713>/Sum' */
1642 localB->VectorConcatenate[2] = localB->u2u3u4 + localB->u1u6;
1643
1644 /* Product: '<S714>/u(2)*u(3)*u(4)' */
1645 localB->u2u3u4 = localB->SinCos_o1[1] * localB->SinCos_o1[2] *
1646 localB->SinCos_o2[0];
1647
1648 /* Product: '<S714>/u(1)*u(6)' */
1649 localB->u1u6 = localB->SinCos_o2[2] * localB->SinCos_o1[0];
1650
1651 /* Sum: '<S714>/Sum' */
1652 localB->VectorConcatenate[5] = localB->u2u3u4 - localB->u1u6;
1653
1654 /* Product: '<S715>/u(4)*u(5)' */
1655 localB->VectorConcatenate[8] = localB->SinCos_o2[0] * localB->SinCos_o2[1];
1656
1657 /* Switch: '<S620>/Switch' */
1658 localB->Switch = (int8_T)(localB->Gain > 3.0);
1659 }
1660}
1661
1662/* Update for atomic system: '<S466>/Nose Gear Geometry Forces and Moments' */
1663void AFCS_MODEL1_NoseGearGeometryForcesandMoments_Update(SimStruct * const S,
1664 B_NoseGearGeometryForcesandMo_T *localB)
1665{
1666 /* Update for Enabled SubSystem: '<S620>/GenerateGroundFM' incorporates:
1667 * Update for EnablePort: '<S686>/Enable'
1668 */
1669 if (((boolean_T *)ssGetDWork(S, 628))[0] && ssIsSampleHit(S, 1, 0)) {
1670 /* Update for Memory: '<S693>/Xcmpr_prev' */
1671 ((real_T *)ssGetDWork(S, 624))[0] = localB->RangeLimit;
1672 }
1673
1674 /* End of Update for SubSystem: '<S620>/GenerateGroundFM' */
1675}
1676
1677/* Termination for atomic system: '<S466>/Nose Gear Geometry Forces and Moments' */
1678void AFCS_MODEL1_NoseGearGeometryForcesandMoments_Term(SimStruct *const S)
1679{
1680}
1681
1682/* System initialize for atomic system: '<S466>/R Main Gear Geometry Forces and Moments' */
1683void AFCS_MODEL1_RMainGearGeometryForcesandMoments_Init(SimStruct * const S,
1684 B_RMainGearGeometryForcesandM_T *localB)
1685{
1686 /* SystemInitialize for Enabled SubSystem: '<S621>/GenerateGroundFM' */
1687 /* InitializeConditions for Memory: '<S727>/Xcmpr_prev' */
1688 ((real_T *)ssGetDWork(S, 615))[0] = 0.0;
1689
1690 /* SystemInitialize for Outport: '<S717>/FM' */
1691 localB->LocalForcestoBodyAxes1[0] = 0.0;
1692 localB->Gain2[0] = 0.0;
1693 localB->LocalForcestoBodyAxes1[1] = 0.0;
1694 localB->Gain2[1] = 0.0;
1695 localB->LocalForcestoBodyAxes1[2] = 0.0;
1696 localB->Gain2[2] = 0.0;
1697
1698 /* SystemInitialize for Outport: '<S717>/GearCmprs' */
1699 localB->RangeLimit = 0.0;
1700
1701 /* End of SystemInitialize for SubSystem: '<S621>/GenerateGroundFM' */
1702}
1703
1704/* Disable for atomic system: '<S466>/R Main Gear Geometry Forces and Moments' */
1705void AFCS_MODEL1_RMainGearGeometryForcesandMoments_Disable(SimStruct * const S,
1706 B_RMainGearGeometryForcesandM_T *localB)
1707{
1708 /* Disable for Enabled SubSystem: '<S621>/GenerateGroundFM' */
1709 if (((boolean_T *)ssGetDWork(S, 623))[0]) {
1710 /* Disable for Outport: '<S717>/FM' */
1711 localB->LocalForcestoBodyAxes1[0] = 0.0;
1712 localB->Gain2[0] = 0.0;
1713 localB->LocalForcestoBodyAxes1[1] = 0.0;
1714 localB->Gain2[1] = 0.0;
1715 localB->LocalForcestoBodyAxes1[2] = 0.0;
1716 localB->Gain2[2] = 0.0;
1717
1718 /* Disable for Outport: '<S717>/GearCmprs' */
1719 localB->RangeLimit = 0.0;
1720 ((boolean_T *)ssGetDWork(S, 623))[0] = false;
1721 }
1722
1723 /* End of Disable for SubSystem: '<S621>/GenerateGroundFM' */
1724}
1725
1726/* Start for atomic system: '<S466>/R Main Gear Geometry Forces and Moments' */
1727void AFCS_MODEL1_RMainGearGeometryForcesandMoments_Start(SimStruct * const S)
1728{
1729 /* Start for Enabled SubSystem: '<S621>/GenerateGroundFM' */
1730 ((boolean_T *)ssGetDWork(S, 623))[0] = false;
1731 ssSetSolverNeedsReset(S);
1732}
1733
1734/* Outputs for atomic system: '<S466>/R Main Gear Geometry Forces and Moments' */
1735void AFCS_MODEL1_RMainGearGeometryForcesandMoments(SimStruct * const S, real_T
1736 rtu_ground_z_m, real_T rtu_plant, real_T rtu_plant_k, real_T rtu_plant_b,
1737 const real_T rtu_GearLoc[3], real_T rtu_FricCoeff, real_T rtu_FricCoeff_k,
1738 const real_T rtu_Euler[3], B_RMainGearGeometryForcesandM_T *localB,
1739 ConstB_RMainGearGeometryForce_T *localC)
1740{
1741 real_T cosOut;
1742 int32_T i;
1743 if (1) {
1744 /* Sum: '<S621>/Subtract' */
1745 localB->Gain = rtu_plant + rtu_ground_z_m;
1746
1747 /* Switch: '<S621>/Switch1' */
1748 localB->Switch1 = (int8_T)!(localB->Gain > 15.0);
1749 }
1750
1751 if (ssIsSampleHit(S, 1, 0)) {
1752 /* SignalConversion: '<S621>/HiddenBuf_InsertedFor_GenerateGroundFM_at_inport_5' */
1753 localB->HiddenBuf_InsertedFor_GenerateG = localB->Switch1;
1754
1755 /* Outputs for Enabled SubSystem: '<S621>/GenerateGroundFM' incorporates:
1756 * EnablePort: '<S717>/Enable'
1757 */
1758 if (ssIsMajorTimeStep(S)) {
1759 if (localB->HiddenBuf_InsertedFor_GenerateG > 0) {
1760 if (!((boolean_T *)ssGetDWork(S, 623))[0]) {
1761 /* Outputs for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:
1762 * EnablePort: '<S651>/Enable'
1763 */
1764 if (ssGetTaskTime(S,1) != ssGetTStart(S)) {
1765 ssSetSolverNeedsReset(S);
1766 }
1767
1768 /* End of Outputs for SubSystem: '<S618>/GenerateGroundFM' */
1769 ((boolean_T *)ssGetDWork(S, 623))[0] = true;
1770 }
1771 } else {
1772 if (((boolean_T *)ssGetDWork(S, 623))[0]) {
1773 ssSetSolverNeedsReset(S);
1774
1775 /* Disable for Outport: '<S717>/FM' */
1776 localB->LocalForcestoBodyAxes1[0] = 0.0;
1777 localB->Gain2[0] = 0.0;
1778 localB->LocalForcestoBodyAxes1[1] = 0.0;
1779 localB->Gain2[1] = 0.0;
1780 localB->LocalForcestoBodyAxes1[2] = 0.0;
1781 localB->Gain2[2] = 0.0;
1782
1783 /* Disable for Outport: '<S717>/GearCmprs' */
1784 localB->RangeLimit = 0.0;
1785 ((boolean_T *)ssGetDWork(S, 623))[0] = false;
1786 }
1787 }
1788 }
1789
1790 /* End of Outputs for SubSystem: '<S621>/GenerateGroundFM' */
1791 }
1792
1793 /* Outputs for Enabled SubSystem: '<S621>/GenerateGroundFM' incorporates:
1794 * EnablePort: '<S717>/Enable'
1795 */
1796 if (((boolean_T *)ssGetDWork(S, 623))[0]) {
1797 if (ssIsSampleHit(S, 1, 0)) {
1798 /* Memory: '<S727>/Xcmpr_prev' */
1799 localB->Xcmpr_prev = ((real_T *)ssGetDWork(S, 615))[0];
1800 }
1801
1802 if (1) {
1803 /* SignalConversion: '<S722>/TmpSignal ConversionAtSinCosInport1' incorporates:
1804 * Constant: '<S717>/Constant'
1805 */
1806 localB->sincos_o2[0] = rtu_plant_k;
1807 localB->sincos_o2[1] = rtu_plant_b;
1808 localB->sincos_o2[2] = 0.0;
1809
1810 /* Trigonometry: '<S722>/SinCos' */
1811 cosOut = cos(localB->sincos_o2[0]);
1812 localB->sincos_o2[0] = sin(localB->sincos_o2[0]);
1813 localB->sincos_o1[0] = cosOut;
1814 cosOut = cos(localB->sincos_o2[1]);
1815 localB->sincos_o2[1] = sin(localB->sincos_o2[1]);
1816 localB->sincos_o1[1] = cosOut;
1817 cosOut = cos(localB->sincos_o2[2]);
1818 localB->sincos_o2[2] = sin(localB->sincos_o2[2]);
1819 localB->sincos_o1[2] = cosOut;
1820
1821 /* Product: '<S728>/u(5)*u(6)' */
1822 localB->VectorConcatenate_o[0] = localB->sincos_o1[1] * localB->sincos_o1
1823 [2];
1824
1825 /* Product: '<S731>/u(6)*u(1)*u(2)' */
1826 localB->kxi = localB->sincos_o1[2] * localB->sincos_o2[0] *
1827 localB->sincos_o2[1];
1828
1829 /* Product: '<S731>/u(3)*u(4)' */
1830 localB->compression = localB->sincos_o2[2] * localB->sincos_o1[0];
1831
1832 /* Sum: '<S731>/Sum' */
1833 localB->VectorConcatenate_o[1] = localB->kxi - localB->compression;
1834
1835 /* Product: '<S734>/u(1)*u(3)' */
1836 localB->compression = localB->sincos_o2[0] * localB->sincos_o2[2];
1837
1838 /* Product: '<S734>/u(2)*u(4)*u(6)' */
1839 localB->kxi = localB->sincos_o2[1] * localB->sincos_o1[0] *
1840 localB->sincos_o1[2];
1841
1842 /* Sum: '<S734>/Sum' */
1843 localB->VectorConcatenate_o[2] = localB->compression + localB->kxi;
1844
1845 /* Product: '<S729>/u(3)*u(5)' */
1846 localB->VectorConcatenate_o[3] = localB->sincos_o2[2] * localB->sincos_o1
1847 [1];
1848
1849 /* Product: '<S732>/u(1)*u(2)*u(3)' */
1850 localB->compression = localB->sincos_o2[0] * localB->sincos_o2[1] *
1851 localB->sincos_o2[2];
1852
1853 /* Product: '<S732>/u(4)*u(6)' */
1854 localB->kxi = localB->sincos_o1[0] * localB->sincos_o1[2];
1855
1856 /* Sum: '<S732>/Sum' */
1857 localB->VectorConcatenate_o[4] = localB->compression + localB->kxi;
1858
1859 /* Product: '<S735>/u(2)*u(3)*u(4)' */
1860 localB->compression = localB->sincos_o2[1] * localB->sincos_o2[2] *
1861 localB->sincos_o1[0];
1862
1863 /* Product: '<S735>/u(1)*u(6)' */
1864 localB->kxi = localB->sincos_o1[2] * localB->sincos_o2[0];
1865
1866 /* Sum: '<S735>/Sum' */
1867 localB->VectorConcatenate_o[5] = localB->compression - localB->kxi;
1868
1869 /* Gain: '<S730>/Gain2' */
1870 localB->VectorConcatenate_o[6] = -localB->sincos_o2[1];
1871
1872 /* Product: '<S733>/u(1)*u(3)' */
1873 localB->VectorConcatenate_o[7] = localB->sincos_o2[0] * localB->sincos_o1
1874 [1];
1875
1876 /* Product: '<S736>/u(4)*u(5)' */
1877 localB->VectorConcatenate_o[8] = localB->sincos_o1[0] * localB->sincos_o1
1878 [1];
1879
1880 /* Selector: '<S720>/get_row3' */
1881 localB->sincos_o1[0] = localB->VectorConcatenate_o[2];
1882 localB->sincos_o1[1] = localB->VectorConcatenate_o[5];
1883 localB->sincos_o1[2] = localB->VectorConcatenate_o[8];
1884
1885 /* Product: '<S720>/prod1' */
1886 localB->compression = rtu_GearLoc[0] * localB->sincos_o1[0];
1887
1888 /* Product: '<S720>/prod2' */
1889 localB->kxi = rtu_GearLoc[1] * localB->sincos_o1[1];
1890
1891 /* Sum: '<S720>/Sum1' */
1892 localB->Sum1 = ((rtu_ground_z_m + rtu_plant) - localB->compression) -
1893 localB->kxi;
1894
1895 /* Product: '<S727>/compression' */
1896 localB->compression = localB->Sum1 / localB->sincos_o1[2];
1897
1898 /* Sum: '<S727>/Sum1' */
1899 localB->kxi = rtu_GearLoc[2] - localB->compression;
1900
1901 /* Saturate: '<S727>/RangeLimit' */
1902 if (localB->kxi > 0.5) {
1903 localB->RangeLimit = 0.5;
1904 } else if (localB->kxi < 0.0) {
1905 localB->RangeLimit = 0.0;
1906 } else {
1907 localB->RangeLimit = localB->kxi;
1908 }
1909
1910 /* End of Saturate: '<S727>/RangeLimit' */
1911
1912 /* Gain: '<S727>/Gain1' */
1913 localB->kxi = 10000.0 * localB->RangeLimit;
1914
1915 /* Sum: '<S727>/dCompression' */
1916 localB->jxk = localB->RangeLimit - localB->Xcmpr_prev;
1917
1918 /* Gain: '<S727>/ComprRate' */
1919 localB->jxk *= 100.0;
1920
1921 /* Gain: '<S727>/Gain2' */
1922 localB->jxk *= 10000.0;
1923
1924 /* Sum: '<S727>/Sum2' */
1925 localB->kxi += localB->jxk;
1926
1927 /* Product: '<S727>/NormalForce' */
1928 localB->kxi /= localB->sincos_o1[2];
1929
1930 /* Saturate: '<S727>/ground only pushes' */
1931 if (localB->kxi <= 0.0) {
1932 localB->kxi = 0.0;
1933 }
1934
1935 /* End of Saturate: '<S727>/ground only pushes' */
1936
1937 /* Gain: '<S727>/reaction' */
1938 localB->kxi = -localB->kxi;
1939
1940 /* Product: '<S717>/Product8' */
1941 localB->GearLocalForcevector1[0] = localB->kxi * rtu_FricCoeff;
1942
1943 /* Product: '<S717>/Product9' */
1944 localB->GearLocalForcevector1[1] = localB->kxi * rtu_FricCoeff_k;
1945
1946 /* SignalConversion: '<S717>/ConcatBufferAtGear Local Force vector1In2' */
1947 localB->GearLocalForcevector1[2] = localB->kxi;
1948
1949 /* Product: '<S717>/Local Forces to Body Axes1' */
1950 for (i = 0; i < 3; i++) {
1951 localB->LocalForcestoBodyAxes1[i] = 0.0;
1952 localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_o[i] *
1953 localB->GearLocalForcevector1[0];
1954 localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_o[i + 3] *
1955 localB->GearLocalForcevector1[1];
1956 localB->LocalForcestoBodyAxes1[i] += localB->VectorConcatenate_o[i + 6] *
1957 localB->GearLocalForcevector1[2];
1958 }
1959
1960 /* End of Product: '<S717>/Local Forces to Body Axes1' */
1961
1962 /* Product: '<S725>/j x k' */
1963 localB->jxk = rtu_GearLoc[1] * localB->LocalForcestoBodyAxes1[2];
1964
1965 /* Product: '<S725>/k x i' */
1966 localB->kxi = rtu_GearLoc[2] * localB->LocalForcestoBodyAxes1[0];
1967
1968 /* Product: '<S725>/i x j' */
1969 localB->ixj = rtu_GearLoc[0] * localB->LocalForcestoBodyAxes1[1];
1970
1971 /* Product: '<S726>/k x j' */
1972 localB->kxj = rtu_GearLoc[2] * localB->LocalForcestoBodyAxes1[1];
1973
1974 /* Product: '<S726>/i x k' */
1975 localB->ixk = rtu_GearLoc[0] * localB->LocalForcestoBodyAxes1[2];
1976
1977 /* Product: '<S726>/j x i' */
1978 localB->jxi = rtu_GearLoc[1] * localB->LocalForcestoBodyAxes1[0];
1979
1980 /* Sum: '<S719>/Sum' */
1981 localB->sincos_o1[0] = localB->jxk - localB->kxj;
1982 localB->sincos_o1[1] = localB->kxi - localB->ixk;
1983 localB->sincos_o1[2] = localB->ixj - localB->jxi;
1984
1985 /* Gain: '<S717>/Gain1' */
1986 localB->Gain1[0] = -localB->sincos_o1[0];
1987 localB->Gain1[1] = -localB->sincos_o1[1];
1988 localB->Gain1[2] = -localB->sincos_o1[2];
1989 }
1990
1991 if (ssIsSampleHit(S, 1, 0)) {
1992 /* Constant: '<S723>/zero1' */
1993 localB->VectorConcatenate_f[0] = 0.0;
1994 }
1995
1996 if (1) {
1997 /* SignalConversion: '<S738>/ConcatBufferAtVector ConcatenateIn2' */
1998 localB->VectorConcatenate_f[1] = localB->compression;
1999
2000 /* SignalConversion: '<S738>/ConcatBufferAtVector ConcatenateIn3' */
2001 localB->VectorConcatenate_f[2] = localC->inv3;
2002
2003 /* Gain: '<S723>/inv1' */
2004 localB->VectorConcatenate_f[3] = -localB->compression;
2005 }
2006
2007 if (ssIsSampleHit(S, 1, 0)) {
2008 /* Constant: '<S723>/zero2' */
2009 localB->VectorConcatenate_f[4] = 0.0;
2010 }
2011
2012 if (1) {
2013 /* SignalConversion: '<S738>/ConcatBufferAtVector ConcatenateIn6' */
2014 localB->VectorConcatenate_f[5] = rtu_GearLoc[0];
2015
2016 /* SignalConversion: '<S738>/ConcatBufferAtVector ConcatenateIn7' */
2017 localB->VectorConcatenate_f[6] = rtu_GearLoc[1];
2018
2019 /* SignalConversion: '<S738>/ConcatBufferAtVector ConcatenateIn8' */
2020 localB->VectorConcatenate_f[7] = localC->inv2;
2021 }
2022
2023 if (ssIsSampleHit(S, 1, 0)) {
2024 /* Constant: '<S723>/zero3' */
2025 localB->VectorConcatenate_f[8] = 0.0;
2026 }
2027
2028 if (1) {
2029 /* Product: '<S721>/force to moment' */
2030 for (i = 0; i < 3; i++) {
2031 localB->forcetomoment[i] = 0.0;
2032 localB->forcetomoment[i] += localB->VectorConcatenate_f[3 * i] *
2033 localB->LocalForcestoBodyAxes1[0];
2034 localB->forcetomoment[i] += localB->VectorConcatenate_f[3 * i + 1] *
2035 localB->LocalForcestoBodyAxes1[1];
2036 localB->forcetomoment[i] += localB->VectorConcatenate_f[3 * i + 2] *
2037 localB->LocalForcestoBodyAxes1[2];
2038 }
2039
2040 /* End of Product: '<S721>/force to moment' */
2041 }
2042
2043 if (ssIsSampleHit(S, 1, 0)) {
2044 }
2045
2046 if (1) {
2047 /* Math: '<S720>/Math Function1' */
2048 for (i = 0; i < 3; i++) {
2049 localB->MathFunction1[3 * i] = localB->VectorConcatenate_o[i];
2050 localB->MathFunction1[1 + 3 * i] = localB->VectorConcatenate_o[i + 3];
2051 localB->MathFunction1[2 + 3 * i] = localB->VectorConcatenate_o[i + 6];
2052 }
2053
2054 /* End of Math: '<S720>/Math Function1' */
2055
2056 /* SignalConversion: '<S720>/TmpSignal ConversionAtProduct3Inport2' incorporates:
2057 * Constant: '<S720>/Constant1'
2058 */
2059 localB->sincos_o1[0] = rtu_GearLoc[0];
2060 localB->sincos_o1[1] = rtu_GearLoc[1];
2061 localB->sincos_o1[2] = 0.0;
2062
2063 /* Product: '<S720>/Product3' */
2064 for (i = 0; i < 3; i++) {
2065 localB->Product3[i] = 0.0;
2066 localB->Product3[i] += localB->MathFunction1[i] * localB->sincos_o1[0];
2067 localB->Product3[i] += localB->MathFunction1[i + 3] * localB->sincos_o1
2068 [1];
2069 localB->Product3[i] += localB->MathFunction1[i + 6] * localB->sincos_o1
2070 [2];
2071 }
2072
2073 /* End of Product: '<S720>/Product3' */
2074
2075 /* Sum: '<S720>/Sum5' */
2076 localB->Sum5 = rtu_plant - localB->Product3[2];
2077 }
2078
2079 if (ssIsSampleHit(S, 1, 0)) {
2080 }
2081
2082 if (1) {
2083 /* Gain: '<S717>/Gain2' */
2084 localB->Gain2[0] = localB->forcetomoment[0];
2085
2086 /* Trigonometry: '<S739>/sincos' */
2087 localB->sincos_o1[0] = sin(rtu_Euler[0]);
2088 localB->sincos_o2[0] = cos(rtu_Euler[0]);
2089
2090 /* Gain: '<S717>/Gain2' */
2091 localB->Gain2[1] = -localB->forcetomoment[1];
2092
2093 /* Trigonometry: '<S739>/sincos' */
2094 localB->sincos_o1[1] = sin(rtu_Euler[1]);
2095 localB->sincos_o2[1] = cos(rtu_Euler[1]);
2096
2097 /* Gain: '<S717>/Gain2' */
2098 localB->Gain2[2] = -localB->forcetomoment[2];
2099
2100 /* Trigonometry: '<S739>/sincos' */
2101 localB->sincos_o1[2] = sin(rtu_Euler[2]);
2102 localB->sincos_o2[2] = cos(rtu_Euler[2]);
2103
2104 /* Fcn: '<S739>/Fcn11' */
2105 localB->VectorConcatenate_h[0] = localB->sincos_o2[1] * localB->sincos_o2
2106 [0];
2107
2108 /* Fcn: '<S739>/Fcn21' */
2109 localB->VectorConcatenate_h[1] = localB->sincos_o1[2] * localB->sincos_o1
2110 [1] * localB->sincos_o2[0] - localB->sincos_o2[2] * localB->sincos_o1[0];
2111
2112 /* Fcn: '<S739>/Fcn31' */
2113 localB->VectorConcatenate_h[2] = localB->sincos_o2[2] * localB->sincos_o1
2114 [1] * localB->sincos_o2[0] + localB->sincos_o1[2] * localB->sincos_o1[0];
2115
2116 /* Fcn: '<S739>/Fcn12' */
2117 localB->VectorConcatenate_h[3] = localB->sincos_o2[1] * localB->sincos_o1
2118 [0];
2119
2120 /* Fcn: '<S739>/Fcn22' */
2121 localB->VectorConcatenate_h[4] = localB->sincos_o1[2] * localB->sincos_o1
2122 [1] * localB->sincos_o1[0] + localB->sincos_o2[2] * localB->sincos_o2[0];
2123
2124 /* Fcn: '<S739>/Fcn32' */
2125 localB->VectorConcatenate_h[5] = localB->sincos_o2[2] * localB->sincos_o1
2126 [1] * localB->sincos_o1[0] - localB->sincos_o1[2] * localB->sincos_o2[0];
2127
2128 /* Fcn: '<S739>/Fcn13' */
2129 localB->VectorConcatenate_h[6] = -localB->sincos_o1[1];
2130
2131 /* Fcn: '<S739>/Fcn23' */
2132 localB->VectorConcatenate_h[7] = localB->sincos_o1[2] * localB->sincos_o2
2133 [1];
2134
2135 /* Fcn: '<S739>/Fcn33' */
2136 localB->VectorConcatenate_h[8] = localB->sincos_o2[2] * localB->sincos_o2
2137 [1];
2138 }
2139 }
2140
2141 /* End of Outputs for SubSystem: '<S621>/GenerateGroundFM' */
2142 if (1) {
2143 /* Gain: '<S621>/Gain' */
2144 localB->Gain = -localB->LocalForcestoBodyAxes1[2];
2145
2146 /* SignalConversion: '<S718>/TmpSignal ConversionAtSinCosInport1' incorporates:
2147 * Constant: '<S621>/Constant'
2148 */
2149 localB->SinCos_o1[0] = rtu_plant_k;
2150 localB->SinCos_o1[1] = rtu_plant_b;
2151 localB->SinCos_o1[2] = 0.0;
2152
2153 /* Trigonometry: '<S718>/SinCos' */
2154 cosOut = cos(localB->SinCos_o1[0]);
2155 localB->SinCos_o1[0] = sin(localB->SinCos_o1[0]);
2156 localB->SinCos_o2[0] = cosOut;
2157 cosOut = cos(localB->SinCos_o1[1]);
2158 localB->SinCos_o1[1] = sin(localB->SinCos_o1[1]);
2159 localB->SinCos_o2[1] = cosOut;
2160 cosOut = cos(localB->SinCos_o1[2]);
2161 localB->SinCos_o1[2] = sin(localB->SinCos_o1[2]);
2162 localB->SinCos_o2[2] = cosOut;
2163
2164 /* Product: '<S741>/u(5)*u(6)' */
2165 localB->VectorConcatenate[0] = localB->SinCos_o2[1] * localB->SinCos_o2[2];
2166
2167 /* Product: '<S742>/u(3)*u(5)' */
2168 localB->VectorConcatenate[3] = localB->SinCos_o1[2] * localB->SinCos_o2[1];
2169
2170 /* Gain: '<S743>/Gain2' */
2171 localB->VectorConcatenate[6] = -localB->SinCos_o1[1];
2172
2173 /* Product: '<S744>/u(6)*u(1)*u(2)' */
2174 localB->u1u6 = localB->SinCos_o2[2] * localB->SinCos_o1[0] *
2175 localB->SinCos_o1[1];
2176
2177 /* Product: '<S744>/u(3)*u(4)' */
2178 localB->u2u3u4 = localB->SinCos_o1[2] * localB->SinCos_o2[0];
2179
2180 /* Sum: '<S744>/Sum' */
2181 localB->VectorConcatenate[1] = localB->u1u6 - localB->u2u3u4;
2182
2183 /* Product: '<S745>/u(1)*u(2)*u(3)' */
2184 localB->u2u3u4 = localB->SinCos_o1[0] * localB->SinCos_o1[1] *
2185 localB->SinCos_o1[2];
2186
2187 /* Product: '<S745>/u(4)*u(6)' */
2188 localB->u1u6 = localB->SinCos_o2[0] * localB->SinCos_o2[2];
2189
2190 /* Sum: '<S745>/Sum' */
2191 localB->VectorConcatenate[4] = localB->u2u3u4 + localB->u1u6;
2192
2193 /* Product: '<S746>/u(1)*u(3)' */
2194 localB->VectorConcatenate[7] = localB->SinCos_o1[0] * localB->SinCos_o2[1];
2195
2196 /* Product: '<S747>/u(1)*u(3)' */
2197 localB->u2u3u4 = localB->SinCos_o1[0] * localB->SinCos_o1[2];
2198
2199 /* Product: '<S747>/u(2)*u(4)*u(6)' */
2200 localB->u1u6 = localB->SinCos_o1[1] * localB->SinCos_o2[0] *
2201 localB->SinCos_o2[2];
2202
2203 /* Sum: '<S747>/Sum' */
2204 localB->VectorConcatenate[2] = localB->u2u3u4 + localB->u1u6;
2205
2206 /* Product: '<S748>/u(2)*u(3)*u(4)' */
2207 localB->u2u3u4 = localB->SinCos_o1[1] * localB->SinCos_o1[2] *
2208 localB->SinCos_o2[0];
2209
2210 /* Product: '<S748>/u(1)*u(6)' */
2211 localB->u1u6 = localB->SinCos_o2[2] * localB->SinCos_o1[0];
2212
2213 /* Sum: '<S748>/Sum' */
2214 localB->VectorConcatenate[5] = localB->u2u3u4 - localB->u1u6;
2215
2216 /* Product: '<S749>/u(4)*u(5)' */
2217 localB->VectorConcatenate[8] = localB->SinCos_o2[0] * localB->SinCos_o2[1];
2218
2219 /* Switch: '<S621>/Switch' */
2220 localB->Switch = (int8_T)(localB->Gain > 3.0);
2221 }
2222}
2223
2224/* Update for atomic system: '<S466>/R Main Gear Geometry Forces and Moments' */
2225void AFCS_MODEL1_RMainGearGeometryForcesandMoments_Update(SimStruct * const S,
2226 B_RMainGearGeometryForcesandM_T *localB)
2227{
2228 /* Update for Enabled SubSystem: '<S621>/GenerateGroundFM' incorporates:
2229 * Update for EnablePort: '<S717>/Enable'
2230 */
2231 if (((boolean_T *)ssGetDWork(S, 623))[0] && ssIsSampleHit(S, 1, 0)) {
2232 /* Update for Memory: '<S727>/Xcmpr_prev' */
2233 ((real_T *)ssGetDWork(S, 615))[0] = localB->RangeLimit;
2234 }
2235
2236 /* End of Update for SubSystem: '<S621>/GenerateGroundFM' */
2237}
2238
2239/* Termination for atomic system: '<S466>/R Main Gear Geometry Forces and Moments' */
2240void AFCS_MODEL1_RMainGearGeometryForcesandMoments_Term(SimStruct *const S)
2241{
2242}
2243
2244static real_T AFCS_MODEL1_rt_roundd_snf(real_T u)
2245{
2246 real_T y;
2247 if (fabs(u) < 4.503599627370496E+15) {
2248 if (u >= 0.5) {
2249 y = floor(u + 0.5);
2250 } else if (u > -0.5) {
2251 y = u * 0.0;
2252 } else {
2253 y = ceil(u - 0.5);
2254 }
2255 } else {
2256 y = u;
2257 }
2258
2259 return y;
2260}
2261
2262static real_T AFCS_MODEL1_rt_remd_snf(real_T u0, real_T u1)
2263{
2264 real_T y;
2265 real_T u1_0;
2266 if (!((!rtIsNaN(u0)) && (!rtIsInf(u0)) && ((!rtIsNaN(u1)) && (!rtIsInf(u1)))))
2267 {
2268 y = (rtNaN);
2269 } else {
2270 if (u1 < 0.0) {
2271 u1_0 = ceil(u1);
2272 } else {
2273 u1_0 = floor(u1);
2274 }
2275
2276 if ((u1 != 0.0) && (u1 != u1_0)) {
2277 u1_0 = u0 / u1;
2278 if (fabs(u1_0 - AFCS_MODEL1_rt_roundd_snf(u1_0)) <= DBL_EPSILON * fabs
2279 (u1_0)) {
2280 y = 0.0;
2281 } else {
2282 y = fmod(u0, u1);
2283 }
2284 } else {
2285 y = fmod(u0, u1);
2286 }
2287 }
2288
2289 return y;
2290}
2291
2292/* Function for MATLAB Function: '<S1>/MATLAB Function1' */
2293static void AFCS_MODEL1_sind_m(real_T *x)
2294{
2295 int8_T n;
2296 static real_T c_x;
2297 static real_T absx;
2298 if (!((!rtIsInf(*x)) && (!rtIsNaN(*x)))) {
2299 c_x = (rtNaN);
2300 } else {
2301 c_x = AFCS_MODEL1_rt_remd_snf(*x, 360.0);
2302 absx = fabs(c_x);
2303 if (absx > 180.0) {
2304 if (c_x > 0.0) {
2305 c_x -= 360.0;
2306 } else {
2307 c_x += 360.0;
2308 }
2309
2310 absx = fabs(c_x);
2311 }
2312
2313 if (absx <= 45.0) {
2314 c_x *= 0.017453292519943295;
2315 n = 0;
2316 } else if (absx <= 135.0) {
2317 if (c_x > 0.0) {
2318 c_x = (c_x - 90.0) * 0.017453292519943295;
2319 n = 1;
2320 } else {
2321 c_x = (c_x + 90.0) * 0.017453292519943295;
2322 n = -1;
2323 }
2324 } else if (c_x > 0.0) {
2325 c_x = (c_x - 180.0) * 0.017453292519943295;
2326 n = 2;
2327 } else {
2328 c_x = (c_x + 180.0) * 0.017453292519943295;
2329 n = -2;
2330 }
2331
2332 if (n == 0) {
2333 c_x = sin(c_x);
2334 } else if (n == 1) {
2335 c_x = cos(c_x);
2336 } else if (n == -1) {
2337 c_x = -cos(c_x);
2338 } else {
2339 c_x = -sin(c_x);
2340 }
2341 }
2342
2343 *x = c_x;
2344}
2345
2346/* Function for MATLAB Function: '<S1>/MATLAB Function1' */
2347static void AFCS_MODEL1_cosd_c(real_T *x)
2348{
2349 int8_T n;
2350 static real_T b_x;
2351 static real_T absx;
2352 if (!((!rtIsInf(*x)) && (!rtIsNaN(*x)))) {
2353 *x = (rtNaN);
2354 } else {
2355 b_x = AFCS_MODEL1_rt_remd_snf(*x, 360.0);
2356 absx = fabs(b_x);
2357 if (absx > 180.0) {
2358 if (b_x > 0.0) {
2359 b_x -= 360.0;
2360 } else {
2361 b_x += 360.0;
2362 }
2363
2364 absx = fabs(b_x);
2365 }
2366
2367 if (absx <= 45.0) {
2368 b_x *= 0.017453292519943295;
2369 n = 0;
2370 } else if (absx <= 135.0) {
2371 if (b_x > 0.0) {
2372 b_x = (b_x - 90.0) * 0.017453292519943295;
2373 n = 1;
2374 } else {
2375 b_x = (b_x + 90.0) * 0.017453292519943295;
2376 n = -1;
2377 }
2378 } else if (b_x > 0.0) {
2379 b_x = (b_x - 180.0) * 0.017453292519943295;
2380 n = 2;
2381 } else {
2382 b_x = (b_x + 180.0) * 0.017453292519943295;
2383 n = -2;
2384 }
2385
2386 if (n == 0) {
2387 *x = cos(b_x);
2388 } else if (n == 1) {
2389 *x = -sin(b_x);
2390 } else if (n == -1) {
2391 *x = sin(b_x);
2392 } else {
2393 *x = -cos(b_x);
2394 }
2395 }
2396}
2397
2398static real_T AFCS_MODEL1_rt_atan2d_snf(real_T u0, real_T u1)
2399{
2400 real_T y;
2401 int32_T u0_0;
2402 int32_T u1_0;
2403 if (rtIsNaN(u0) || rtIsNaN(u1)) {
2404 y = (rtNaN);
2405 } else if (rtIsInf(u0) && rtIsInf(u1)) {
2406 if (u0 > 0.0) {
2407 u0_0 = 1;
2408 } else {
2409 u0_0 = -1;
2410 }
2411
2412 if (u1 > 0.0) {
2413 u1_0 = 1;
2414 } else {
2415 u1_0 = -1;
2416 }
2417
2418 y = atan2(u0_0, u1_0);
2419 } else if (u1 == 0.0) {
2420 if (u0 > 0.0) {
2421 y = RT_PI / 2.0;
2422 } else if (u0 < 0.0) {
2423 y = -(RT_PI / 2.0);
2424 } else {
2425 y = 0.0;
2426 }
2427 } else {
2428 y = atan2(u0, u1);
2429 }
2430
2431 return y;
2432}
2433
2434static real_T AFCS_MODEL1_rt_modd_snf(real_T u0, real_T u1)
2435{
2436 real_T y;
2437 real_T tmp;
2438 if (u1 == 0.0) {
2439 y = u0;
2440 } else if (!((!rtIsNaN(u0)) && (!rtIsInf(u0)) && ((!rtIsNaN(u1)) && (!rtIsInf
2441 (u1))))) {
2442 y = (rtNaN);
2443 } else {
2444 tmp = u0 / u1;
2445 if (u1 <= floor(u1)) {
2446 y = u0 - floor(tmp) * u1;
2447 } else {
2448 if (fabs(tmp - AFCS_MODEL1_rt_roundd_snf(tmp)) <= DBL_EPSILON * fabs(tmp))
2449 {
2450 y = 0.0;
2451 } else {
2452 y = (tmp - floor(tmp)) * u1;
2453 }
2454 }
2455 }
2456
2457 return y;
2458}
2459
2460static real32_T AFCS_MODEL1_rt_atan2f_snf(real32_T u0, real32_T u1)
2461{
2462 real32_T y;
2463 int32_T u0_0;
2464 int32_T u1_0;
2465 if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
2466 y = (rtNaNF);
2467 } else if (rtIsInfF(u0) && rtIsInfF(u1)) {
2468 if (u0 > 0.0F) {
2469 u0_0 = 1;
2470 } else {
2471 u0_0 = -1;
2472 }
2473
2474 if (u1 > 0.0F) {
2475 u1_0 = 1;
2476 } else {
2477 u1_0 = -1;
2478 }
2479
2480 y = (real32_T)atan2((real32_T)u0_0, (real32_T)u1_0);
2481 } else if (u1 == 0.0F) {
2482 if (u0 > 0.0F) {
2483 y = RT_PIF / 2.0F;
2484 } else if (u0 < 0.0F) {
2485 y = -(RT_PIF / 2.0F);
2486 } else {
2487 y = 0.0F;
2488 }
2489 } else {
2490 y = (real32_T)atan2(u0, u1);
2491 }
2492
2493 return y;
2494}
2495
2496/* Function for Chart: '<S21>/delayblock3' */
2497static int32_T AFCS_MODEL1_addup_g(int32_T bA, int32_T iB, int32_T iC)
2498{
2499 int32_T tC;
2500 int64_T tmp;
2501
2502 /* MATLAB Function 'addup': '<S50>:114' */
2503 /* Graphical Function 'addup': '<S50>:114' */
2504 /* '<S50>:122:1' */
2505 tC = iC;
2506
2507 /* '<S50>:123:1' */
2508 if (bA == 0) {
2509 /* '<S50>:126:1' */
2510 if (iC > 0) {
2511 /* '<S50>:126:1' */
2512 tC = iC - 1;
2513 }
2514 } else {
2515 /* '<S50>:128:1' */
2516 if (iC > iB) {
2517 /* '<S50>:128:1' */
2518 tC = iB;
2519 } else {
2520 /* '<S50>:127:1' */
2521 tmp = iC + 1LL;
2522 if (tmp > 2147483647LL) {
2523 tmp = 2147483647LL;
2524 } else {
2525 if (tmp < -2147483648LL) {
2526 tmp = -2147483648LL;
2527 }
2528 }
2529
2530 tC = (int32_T)tmp;
2531 }
2532 }
2533
2534 return tC;
2535}
2536
2537/* Function for MATLAB Function: '<S293>/MATLAB Function3' */
2538static void AFCS_MODEL1_GetPsi(real_T A_lon, real_T A_lat, real_T B_lon, real_T
2539 B_lat, real_T *Drange, real_T *PsiNorth)
2540{
2541 static real_T lona;
2542 static real_T lata;
2543 static real_T lonb;
2544 static real_T latb;
2545 static real_T a;
2546 real_T b_a;
2547
2548 /* '<S298>:1:53' */
2549 /* '<S298>:1:54' */
2550 /* '<S298>:1:55' */
2551 lona = A_lon * 0.017453292519943295;
2552
2553 /* '<S298>:1:56' */
2554 lata = A_lat * 0.017453292519943295;
2555
2556 /* '<S298>:1:57' */
2557 lonb = B_lon * 0.017453292519943295;
2558
2559 /* '<S298>:1:58' */
2560 latb = B_lat * 0.017453292519943295;
2561
2562 /* '<S298>:1:59' */
2563 a = latb - lata;
2564 b_a = cos((lata + latb) * 0.5) * (lonb - lona);
2565
2566 /* '<S298>:1:60' */
2567 *Drange = sqrt(a * a + b_a * b_a) * 6.371E+6;
2568
2569 /* %%%%%%%%%%%%%%%计算AB与北向夹角 */
2570 /* '<S298>:1:62' */
2571 /* '<S298>:1:63' */
2572 /* '<S298>:1:64' */
2573 *PsiNorth = AFCS_MODEL1_rt_atan2d_snf(cos((latb + lata) * 0.5) * (lonb - lona),
2574 latb - lata);
2575}
2576
2577static real_T AFCS_MODEL1_rt_powd_snf(real_T u0, real_T u1)
2578{
2579 real_T y;
2580 real_T tmp;
2581 real_T tmp_0;
2582 if (rtIsNaN(u0) || rtIsNaN(u1)) {
2583 y = (rtNaN);
2584 } else {
2585 tmp = fabs(u0);
2586 tmp_0 = fabs(u1);
2587 if (rtIsInf(u1)) {
2588 if (tmp == 1.0) {
2589 y = (rtNaN);
2590 } else if (tmp > 1.0) {
2591 if (u1 > 0.0) {
2592 y = (rtInf);
2593 } else {
2594 y = 0.0;
2595 }
2596 } else if (u1 > 0.0) {
2597 y = 0.0;
2598 } else {
2599 y = (rtInf);
2600 }
2601 } else if (tmp_0 == 0.0) {
2602 y = 1.0;
2603 } else if (tmp_0 == 1.0) {
2604 if (u1 > 0.0) {
2605 y = u0;
2606 } else {
2607 y = 1.0 / u0;
2608 }
2609 } else if (u1 == 2.0) {
2610 y = u0 * u0;
2611 } else if ((u1 == 0.5) && (u0 >= 0.0)) {
2612 y = sqrt(u0);
2613 } else if ((u0 < 0.0) && (u1 > floor(u1))) {
2614 y = (rtNaN);
2615 } else {
2616 y = pow(u0, u1);
2617 }
2618 }
2619
2620 return y;
2621}
2622
2623/* Function for Chart: '<S21>/delayblock2' */
2624static int32_T AFCS_MODEL1_addup_l(int32_T bA, int32_T iB, int32_T iC)
2625{
2626 int32_T tC;
2627 int64_T tmp;
2628
2629 /* MATLAB Function 'addup': '<S49>:114' */
2630 /* Graphical Function 'addup': '<S49>:114' */
2631 /* '<S49>:122:1' */
2632 tC = iC;
2633
2634 /* '<S49>:123:1' */
2635 if (bA == 0) {
2636 /* '<S49>:126:1' */
2637 if (iC > 0) {
2638 /* '<S49>:126:1' */
2639 tC = iC - 1;
2640 }
2641 } else {
2642 /* '<S49>:128:1' */
2643 if (iC > iB) {
2644 /* '<S49>:128:1' */
2645 tC = iB;
2646 } else {
2647 /* '<S49>:127:1' */
2648 tmp = iC + 1LL;
2649 if (tmp > 2147483647LL) {
2650 tmp = 2147483647LL;
2651 } else {
2652 if (tmp < -2147483648LL) {
2653 tmp = -2147483648LL;
2654 }
2655 }
2656
2657 tC = (int32_T)tmp;
2658 }
2659 }
2660
2661 return tC;
2662}
2663
2664/* Function for Chart: '<S32>/wowcmp1' */
2665static int32_T AFCS_MODEL1_addup_p(boolean_T bA, int32_T iB, int32_T iC)
2666{
2667 int32_T tC;
2668 int64_T tmp;
2669
2670 /* MATLAB Function 'addup': '<S331>:114' */
2671 /* Graphical Function 'addup': '<S331>:114' */
2672 /* '<S331>:122:1' */
2673 tC = iC;
2674
2675 /* '<S331>:123:1' */
2676 if (!bA) {
2677 /* '<S331>:126:1' */
2678 if (iC > 0) {
2679 /* '<S331>:126:1' */
2680 tC = iC - 1;
2681 }
2682 } else {
2683 /* '<S331>:128:1' */
2684 if (iC > iB) {
2685 /* '<S331>:128:1' */
2686 tC = iB;
2687 } else {
2688 /* '<S331>:127:1' */
2689 tmp = iC + 1LL;
2690 if (tmp > 2147483647LL) {
2691 tmp = 2147483647LL;
2692 } else {
2693 if (tmp < -2147483648LL) {
2694 tmp = -2147483648LL;
2695 }
2696 }
2697
2698 tC = (int32_T)tmp;
2699 }
2700 }
2701
2702 return tC;
2703}
2704
2705/* Function for Chart: '<S21>/delayblock4' */
2706static int32_T AFCS_MODEL1_addup_i(int32_T bA, int32_T iB, int32_T iC)
2707{
2708 int32_T tC;
2709 int64_T tmp;
2710
2711 /* MATLAB Function 'addup': '<S51>:114' */
2712 /* Graphical Function 'addup': '<S51>:114' */
2713 /* '<S51>:122:1' */
2714 tC = iC;
2715
2716 /* '<S51>:123:1' */
2717 if (bA == 0) {
2718 /* '<S51>:126:1' */
2719 if (iC > 0) {
2720 /* '<S51>:126:1' */
2721 tC = iC - 1;
2722 }
2723 } else {
2724 /* '<S51>:128:1' */
2725 if (iC > iB) {
2726 /* '<S51>:128:1' */
2727 tC = iB;
2728 } else {
2729 /* '<S51>:127:1' */
2730 tmp = iC + 1LL;
2731 if (tmp > 2147483647LL) {
2732 tmp = 2147483647LL;
2733 } else {
2734 if (tmp < -2147483648LL) {
2735 tmp = -2147483648LL;
2736 }
2737 }
2738
2739 tC = (int32_T)tmp;
2740 }
2741 }
2742
2743 return tC;
2744}
2745
2746/* Function for Chart: '<S21>/delayblock5' */
2747static int32_T AFCS_MODEL1_addup_k(int32_T bA, int32_T iB, int32_T iC)
2748{
2749 int32_T tC;
2750 int64_T tmp;
2751
2752 /* MATLAB Function 'addup': '<S52>:114' */
2753 /* Graphical Function 'addup': '<S52>:114' */
2754 /* '<S52>:122:1' */
2755 tC = iC;
2756
2757 /* '<S52>:123:1' */
2758 if (bA == 0) {
2759 /* '<S52>:126:1' */
2760 if (iC > 0) {
2761 /* '<S52>:126:1' */
2762 tC = iC - 1;
2763 }
2764 } else {
2765 /* '<S52>:128:1' */
2766 if (iC > iB) {
2767 /* '<S52>:128:1' */
2768 tC = iB;
2769 } else {
2770 /* '<S52>:127:1' */
2771 tmp = iC + 1LL;
2772 if (tmp > 2147483647LL) {
2773 tmp = 2147483647LL;
2774 } else {
2775 if (tmp < -2147483648LL) {
2776 tmp = -2147483648LL;
2777 }
2778 }
2779
2780 tC = (int32_T)tmp;
2781 }
2782 }
2783
2784 return tC;
2785}
2786
2787/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */
2788static void AFCS_MODEL1_TransformLLATBDX(real_T Lon, real_T Lat, real_T Alt,
2789 real_T Lon0, real_T Lat0, real_T Alt0, real_T *pn, real_T *pe, real_T *pd)
2790{
2791 static real_T m_dE_LN[9];
2792 static real_T A[3];
2793 int32_T i;
2794 static real_T Alt_idx_0;
2795 static real_T Alt_idx_1;
2796 real_T Alt_idx_2;
2797
2798 /* TransformLLATBDX 此处显示有关此函数的摘要 */
2799 /* 将经纬高转换到北东下坐标系 */
2800 /* //1.将经纬高转换为在地心坐标系下x,y,z坐标 */
2801 /* TransformLLATDX 此处显示有关此函数的摘要 */
2802 /* 将经纬高转换到地心坐标系 */
2803 /* //2.将地心坐标系下x,y,z坐标转换为在北天东标系下x,y,z坐标 */
2804 /* UNTITLED4 此处显示有关此函数的摘要 */
2805 /* 将地心坐标系转换到北天东系 */
2806 /* 地心系到北天东系转换矩阵 */
2807 /* TransformLLATDX 此处显示有关此函数的摘要 */
2808 /* 将经纬高转换到地心坐标系 */
2809 m_dE_LN[0] = -sin(Lat0) * cos(Lon0);
2810 m_dE_LN[3] = -sin(Lat0) * sin(Lon0);
2811 m_dE_LN[6] = cos(Lat0);
2812 m_dE_LN[1] = cos(Lat0) * cos(Lon0);
2813 m_dE_LN[4] = cos(Lat0) * sin(Lon0);
2814 m_dE_LN[7] = sin(Lat0);
2815 m_dE_LN[2] = -sin(Lon0);
2816 m_dE_LN[5] = cos(Lon0);
2817 m_dE_LN[8] = 0.0;
2818 Alt_idx_0 = (Alt + 6.378137E+6) * cos(Lat) * cos(Lon) - (Alt0 + 6.378137E+6) *
2819 cos(Lat0) * cos(Lon0);
2820 Alt_idx_1 = (Alt + 6.378137E+6) * cos(Lat) * sin(Lon) - (Alt0 + 6.378137E+6) *
2821 cos(Lat0) * sin(Lon0);
2822 Alt_idx_2 = (Alt + 6.378137E+6) * sin(Lat) - (Alt0 + 6.378137E+6) * sin(Lat0);
2823 for (i = 0; i < 3; i++) {
2824 A[i] = m_dE_LN[i + 6] * Alt_idx_2 + (m_dE_LN[i + 3] * Alt_idx_1 + m_dE_LN[i]
2825 * Alt_idx_0);
2826 }
2827
2828 /* //3.将北天东坐标系下x,y,z坐标转换为在北东下标系下x,y,z坐标 */
2829 /* UNTITLED6 此处显示有关此函数的摘要 */
2830 /* 将北天东系转换到北东下坐标系 */
2831 *pn = A[0];
2832 *pe = A[2];
2833 *pd = -A[1];
2834}
2835
2836/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */
2837static void AFCS_MODEL1_linspace(real_T y[32])
2838{
2839 memset(&y[0], 0, sizeof(real_T) << 5U);
2840}
2841
2842/* Function for Chart: '<S21>/delayblock7' */
2843static int32_T AFCS_MODEL1_addup_h(int32_T bA, int32_T iB, int32_T iC)
2844{
2845 int32_T tC;
2846 int64_T tmp;
2847
2848 /* MATLAB Function 'addup': '<S54>:114' */
2849 /* Graphical Function 'addup': '<S54>:114' */
2850 /* '<S54>:122:1' */
2851 tC = iC;
2852
2853 /* '<S54>:123:1' */
2854 if (bA == 0) {
2855 /* '<S54>:126:1' */
2856 if (iC > 0) {
2857 /* '<S54>:126:1' */
2858 tC = iC - 1;
2859 }
2860 } else {
2861 /* '<S54>:128:1' */
2862 if (iC > iB) {
2863 /* '<S54>:128:1' */
2864 tC = iB;
2865 } else {
2866 /* '<S54>:127:1' */
2867 tmp = iC + 1LL;
2868 if (tmp > 2147483647LL) {
2869 tmp = 2147483647LL;
2870 } else {
2871 if (tmp < -2147483648LL) {
2872 tmp = -2147483648LL;
2873 }
2874 }
2875
2876 tC = (int32_T)tmp;
2877 }
2878 }
2879
2880 return tC;
2881}
2882
2883static real32_T AFCS_MODEL1_rt_roundf_snf(real32_T u)
2884{
2885 real32_T y;
2886 if ((real32_T)fabs(u) < 8.388608E+6F) {
2887 if (u >= 0.5F) {
2888 y = (real32_T)floor(u + 0.5F);
2889 } else if (u > -0.5F) {
2890 y = u * 0.0F;
2891 } else {
2892 y = (real32_T)ceil(u - 0.5F);
2893 }
2894 } else {
2895 y = u;
2896 }
2897
2898 return y;
2899}
2900
2901static real32_T AFCS_MODEL1_rt_remf_snf(real32_T u0, real32_T u1)
2902{
2903 real32_T y;
2904 real32_T u1_0;
2905 if (!((!rtIsNaNF(u0)) && (!rtIsInfF(u0)) && ((!rtIsNaNF(u1)) && (!rtIsInfF(u1)))))
2906 {
2907 y = (rtNaNF);
2908 } else {
2909 if (u1 < 0.0F) {
2910 u1_0 = (real32_T)ceil(u1);
2911 } else {
2912 u1_0 = (real32_T)floor(u1);
2913 }
2914
2915 if ((u1 != 0.0F) && (u1 != u1_0)) {
2916 u1_0 = u0 / u1;
2917 if ((real32_T)fabs(u1_0 - AFCS_MODEL1_rt_roundf_snf(u1_0)) <= FLT_EPSILON *
2918 (real32_T)fabs(u1_0)) {
2919 y = 0.0F;
2920 } else {
2921 y = (real32_T)fmod(u0, u1);
2922 }
2923 } else {
2924 y = (real32_T)fmod(u0, u1);
2925 }
2926 }
2927
2928 return y;
2929}
2930
2931/* Function for Chart: '<S321>/Takeoff' */
2932static int32_T AFCS_MODEL1_addup_lc(boolean_T bA, int32_T iB, int32_T iC)
2933{
2934 int32_T tC;
2935 int64_T tmp;
2936
2937 /* MATLAB Function 'addup': '<S329>:121' */
2938 /* Graphical Function 'addup': '<S329>:121' */
2939 /* '<S329>:129:1' */
2940 tC = iC;
2941
2942 /* '<S329>:130:1' */
2943 if (!bA) {
2944 /* '<S329>:133:1' */
2945 if (iC > 0) {
2946 /* '<S329>:133:1' */
2947 tC = iC - 1;
2948 }
2949 } else {
2950 /* '<S329>:135:1' */
2951 if (iC > iB) {
2952 /* '<S329>:135:1' */
2953 tC = iB;
2954 } else {
2955 /* '<S329>:134:1' */
2956 tmp = iC + 1LL;
2957 if (tmp > 2147483647LL) {
2958 tmp = 2147483647LL;
2959 } else {
2960 if (tmp < -2147483648LL) {
2961 tmp = -2147483648LL;
2962 }
2963 }
2964
2965 tC = (int32_T)tmp;
2966 }
2967 }
2968
2969 return tC;
2970}
2971
2972/* Function for Chart: '<S321>/Takeoff' */
2973static void AFCS_MODEL1_NormalTakeoff(SimStruct *S)
2974{
2975 boolean_T StopTakeoff1FG;
2976 int8_T n;
2977 real32_T b_x;
2978 real32_T absx;
2979 boolean_T guard1 = false;
2980 boolean_T guard2 = false;
2981 B_AFCS_MODEL1_T *_rtB;
2982 _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));
2983
2984 /* During 'NormalTakeoff': '<S329>:136' */
2985 if (((int32_T *)ssGetDWork(S, 326))[0] == 5) {
2986 /* Transition: '<S329>:151' */
2987 /* Exit Internal 'NormalTakeoff': '<S329>:136' */
2988 ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
2989 ((uint8_T *)ssGetDWork(S, 578))[0] = AFCS_MODEL1_IN_AbortTakeoff;
2990
2991 /* Entry 'AbortTakeoff': '<S329>:152' */
2992 _rtB->fmTOtoTaxi = 1;
2993 _rtB->AbortTakeoffFg = 1;
2994 } else {
2995 if (_rtB->Vc_p < 30.0F) {
2996 if ((real32_T)fabs(_rtB->yrwcmd_m - _rtB->yrw) > 8.0F) {
2997 guard1 = true;
2998 } else {
2999 if (!((!rtIsInfF(_rtB->psd)) && (!rtIsNaNF(_rtB->psd)))) {
3000 b_x = (rtNaNF);
3001 } else {
3002 b_x = AFCS_MODEL1_rt_remf_snf(_rtB->psd, 360.0F);
3003 absx = (real32_T)fabs(b_x);
3004 if (absx > 180.0F) {
3005 if (b_x > 0.0F) {
3006 b_x -= 360.0F;
3007 } else {
3008 b_x += 360.0F;
3009 }
3010
3011 absx = (real32_T)fabs(b_x);
3012 }
3013
3014 if (absx <= 45.0F) {
3015 b_x *= 0.0174532924F;
3016 n = 0;
3017 } else if (absx <= 135.0F) {
3018 if (b_x > 0.0F) {
3019 b_x = (b_x - 90.0F) * 0.0174532924F;
3020 n = 1;
3021 } else {
3022 b_x = (b_x + 90.0F) * 0.0174532924F;
3023 n = -1;
3024 }
3025 } else if (b_x > 0.0F) {
3026 b_x = (b_x - 180.0F) * 0.0174532924F;
3027 n = 2;
3028 } else {
3029 b_x = (b_x + 180.0F) * 0.0174532924F;
3030 n = -2;
3031 }
3032
3033 if (n == 0) {
3034 b_x = (real32_T)sin(b_x);
3035 } else if (n == 1) {
3036 b_x = (real32_T)cos(b_x);
3037 } else if (n == -1) {
3038 b_x = -(real32_T)cos(b_x);
3039 } else {
3040 b_x = -(real32_T)sin(b_x);
3041 }
3042 }
3043
3044 if (((real32_T)fabs(_rtB->vt * b_x) > 3.0F) || (_rtB->Switch3 == 1)) {
3045 guard1 = true;
3046 } else {
3047 guard2 = true;
3048 }
3049 }
3050 } else {
3051 guard2 = true;
3052 }
3053
3054 if (guard2) {
3055 /* Constant: '<S17>/Constant18' */
3056 if (_rtB->DtoStart > 1000.0F) {
3057 guard1 = true;
3058 } else {
3059 StopTakeoff1FG = false;
3060 }
3061
3062 /* End of Constant: '<S17>/Constant18' */
3063 }
3064
3065 if (guard1) {
3066 StopTakeoff1FG = true;
3067 }
3068
3069 ((int32_T *)ssGetDWork(S, 326))[0] = AFCS_MODEL1_addup_lc(StopTakeoff1FG, 5,
3070 ((int32_T *)ssGetDWork(S, 326))[0]);
3071 switch (((uint8_T *)ssGetDWork(S, 579))[0]) {
3072 case AFCS_MODEL1_IN_DrControl:
3073 /* During 'DrControl': '<S329>:144' */
3074 if (((int32_T *)ssGetDWork(S, 322))[0] == 5) {
3075 /* Transition: '<S329>:138' */
3076 ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_LiftFrontWheel;
3077
3078 /* Entry 'LiftFrontWheel': '<S329>:139' */
3079 _rtB->apthL_i2 = 1;
3080 _rtB->aplonL_ik = 2;
3081 _rtB->aplatL_d = 2;
3082 _rtB->apyawL_g = 3;
3083 _rtB->apnwsL_g = 2;
3084 _rtB->phiLcmd_h = 0.0F;
3085
3086 /* Constant: '<S17>/Constant1' */
3087 _rtB->thetaLcmd_ae = 7.0F;
3088 ((int32_T *)ssGetDWork(S, 324))[0] = 0;
3089 _rtB->LiftWheelFg = 1;
3090 } else {
3091 /* Constant: '<S17>/Constant8' */
3092 ((int32_T *)ssGetDWork(S, 322))[0] = AFCS_MODEL1_addup_lc(_rtB->Vc_p >
3093 30.0F, 5, ((int32_T *)ssGetDWork(S, 322))[0]);
3094 }
3095 break;
3096
3097 case AFCS_MODEL1_IN_Initial:
3098 /* During 'Initial': '<S329>:193' */
3099 if (((int32_T *)ssGetDWork(S, 327))[0] == 5) {
3100 /* Transition: '<S329>:142' */
3101 ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_NwsControl;
3102
3103 /* Entry 'NwsControl': '<S329>:143' */
3104 _rtB->apthL_i2 = 1;
3105 _rtB->aplonL_ik = 2;
3106 _rtB->aplatL_d = 2;
3107 _rtB->apyawL_g = 1;
3108 _rtB->apnwsL_g = 2;
3109 _rtB->phiLcmd_h = 0.0F;
3110 _rtB->thetaLcmd_ae = -10.0F;
3111 ((int32_T *)ssGetDWork(S, 323))[0] = 0;
3112 } else {
3113 ((int32_T *)ssGetDWork(S, 327))[0] = AFCS_MODEL1_addup_lc(_rtB->vt >
3114 0.5F, 5, ((int32_T *)ssGetDWork(S, 327))[0]);
3115 }
3116 break;
3117
3118 case AFCS_MODEL1_IN_LiftFrontWheel:
3119 /* During 'LiftFrontWheel': '<S329>:139' */
3120 if (((int32_T *)ssGetDWork(S, 324))[0] == 5) {
3121 /* Transition: '<S329>:149' */
3122 ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_StartLatControl;
3123
3124 /* Entry 'StartLatControl': '<S329>:167' */
3125 _rtB->apthL_i2 = 1;
3126 _rtB->aplonL_ik = 2;
3127 _rtB->aplatL_d = 5;
3128 _rtB->apyawL_g = 1;
3129 _rtB->apnwsL_g = 1;
3130 _rtB->phiLcmd_h = 0.0F;
3131 _rtB->thetaLcmd_ae = 7.0F;
3132 ((int32_T *)ssGetDWork(S, 325))[0] = 0;
3133 } else {
3134 ((int32_T *)ssGetDWork(S, 324))[0] = AFCS_MODEL1_addup_lc(_rtB->mh_j >
3135 8.0F, 5, ((int32_T *)ssGetDWork(S, 324))[0]);
3136 }
3137 break;
3138
3139 case AFCS_MODEL1_IN_NwsControl:
3140 /* During 'NwsControl': '<S329>:143' */
3141 if (((int32_T *)ssGetDWork(S, 323))[0] == 5) {
3142 /* Transition: '<S329>:141' */
3143 ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_DrControl;
3144
3145 /* Entry 'DrControl': '<S329>:144' */
3146 _rtB->apthL_i2 = 1;
3147 _rtB->aplonL_ik = 2;
3148 _rtB->aplatL_d = 2;
3149 _rtB->apyawL_g = 3;
3150 _rtB->apnwsL_g = 2;
3151 _rtB->phiLcmd_h = 0.0F;
3152 _rtB->thetaLcmd_ae = -2.0F;
3153 ((int32_T *)ssGetDWork(S, 322))[0] = 0;
3154 } else {
3155 /* Constant: '<S17>/Constant7' */
3156 ((int32_T *)ssGetDWork(S, 323))[0] = AFCS_MODEL1_addup_lc(_rtB->Vc_p >
3157 5.0F, 5, ((int32_T *)ssGetDWork(S, 323))[0]);
3158 }
3159 break;
3160
3161 case AFCS_MODEL1_IN_StartLatControl:
3162 /* During 'StartLatControl': '<S329>:167' */
3163 if (((int32_T *)ssGetDWork(S, 325))[0] == 5) {
3164 /* Transition: '<S329>:168' */
3165 ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_TransClimb;
3166
3167 /* Entry 'TransClimb': '<S329>:140' */
3168 _rtB->fmTOtoClimb = 1;
3169 } else {
3170 ((int32_T *)ssGetDWork(S, 325))[0] = AFCS_MODEL1_addup_lc(_rtB->mh_j >
3171 20.0F, 5, ((int32_T *)ssGetDWork(S, 325))[0]);
3172 }
3173 break;
3174
3175 default:
3176 /* During 'TransClimb': '<S329>:140' */
3177 break;
3178 }
3179 }
3180}
3181
3182/* Function for Chart: '<S316>/Landing2' */
3183static int32_T AFCS_MODEL1_addup_jz(boolean_T bA, int32_T iB, int32_T iC)
3184{
3185 int32_T tC;
3186 int64_T tmp;
3187
3188 /* MATLAB Function 'addup': '<S326>:251' */
3189 /* Graphical Function 'addup': '<S326>:251' */
3190 /* '<S326>:259:1' */
3191 tC = iC;
3192
3193 /* '<S326>:260:1' */
3194 if (!bA) {
3195 /* '<S326>:263:1' */
3196 if (iC > 0) {
3197 /* '<S326>:263:1' */
3198 tC = iC - 1;
3199 }
3200 } else {
3201 /* '<S326>:265:1' */
3202 if (iC > iB) {
3203 /* '<S326>:265:1' */
3204 tC = iB;
3205 } else {
3206 /* '<S326>:264:1' */
3207 tmp = iC + 1LL;
3208 if (tmp > 2147483647LL) {
3209 tmp = 2147483647LL;
3210 } else {
3211 if (tmp < -2147483648LL) {
3212 tmp = -2147483648LL;
3213 }
3214 }
3215
3216 tC = (int32_T)tmp;
3217 }
3218 }
3219
3220 return tC;
3221}
3222
3223/* Function for Chart: '<S316>/Landing2' */
3224static real32_T AFCS_MODEL1_res180_m(real32_T angle)
3225{
3226 real32_T outangle;
3227
3228 /* MATLAB Function 'res180': '<S326>:175' */
3229 /* Graphical Function 'res180': '<S326>:175' */
3230 /* '<S326>:184:1' */
3231 if (angle > 180.0F) {
3232 /* '<S326>:184:1' */
3233 outangle = angle - 360.0F;
3234 } else {
3235 /* '<S326>:186:1' */
3236 if (angle < -180.0F) {
3237 /* '<S326>:186:1' */
3238 outangle = angle + 360.0F;
3239 } else {
3240 /* '<S326>:191:1' */
3241 outangle = angle;
3242 }
3243 }
3244
3245 return outangle;
3246}
3247
3248/* Function for Chart: '<S316>/Landing2' */
3249static void AFCS_MODEL1_Pre_Land(SimStruct *S)
3250{
3251 B_AFCS_MODEL1_T *_rtB;
3252 _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));
3253
3254 /* During 'Pre_Land': '<S326>:357' */
3255 if (((int32_T *)ssGetDWork(S, 301))[0] == 5) {
3256 /* Transition: '<S326>:358' */
3257 /* Exit Internal 'Pre_Land': '<S326>:357' */
3258 /* Exit Internal 'XYPositionControl': '<S326>:367' */
3259 ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3260
3261 /* Exit Internal 'YrwControl1': '<S326>:369' */
3262 ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3263 ((uint8_T *)ssGetDWork(S, 537))[0] = 0U;
3264 ((uint8_T *)ssGetDWork(S, 535))[0] = 0U;
3265 ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_Decline;
3266
3267 /* Entry 'Decline': '<S326>:352' */
3268 ((int32_T *)ssGetDWork(S, 302))[0] = 0;
3269 } else if ((_rtB->GoaroundFg == 1) && (_rtB->mh_j > 30.0F) && (((int32_T *)
3270 ssGetDWork(S, 308))[0] == 0)) {
3271 /* Transition: '<S326>:402' */
3272 /* Exit Internal 'Pre_Land': '<S326>:357' */
3273 /* Exit Internal 'XYPositionControl': '<S326>:367' */
3274 ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3275
3276 /* Exit Internal 'YrwControl1': '<S326>:369' */
3277 ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3278 ((uint8_T *)ssGetDWork(S, 537))[0] = 0U;
3279 ((uint8_T *)ssGetDWork(S, 535))[0] = 0U;
3280 ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_GoAround;
3281
3282 /* Entry Internal 'GoAround': '<S326>:386' */
3283 /* Transition: '<S326>:388' */
3284 ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_Climb;
3285
3286 /* Entry 'Climb': '<S326>:390' */
3287 _rtB->apthL_e = 3;
3288 _rtB->aplonL_f = 4;
3289 _rtB->aplatL_l = 2;
3290 _rtB->apyawL_m = 1;
3291 ((int32_T *)ssGetDWork(S, 307))[0] = 0;
3292 } else {
3293 _rtB->aplonL_f = 4;
3294 _rtB->hcmd_j = _rtB->HrefAL;
3295 _rtB->gamacmd_k = 0.0F;
3296 _rtB->apthL_e = 3;
3297
3298 /* Constant: '<S17>/Constant6' */
3299 _rtB->vtcmd_f = 40.0F;
3300 ((int32_T *)ssGetDWork(S, 301))[0] = AFCS_MODEL1_addup_jz((_rtB->HrefAL >
3301 ((real32_T *)ssGetDWork(S, 286))[0]) && ((real32_T)fabs(_rtB->mh_j -
3302 _rtB->HrefAL) < 40.0F) && ((real32_T)fabs(_rtB->yrw) < 100.0F) &&
3303 (_rtB->GPSFail == 0), 5, ((int32_T *)ssGetDWork(S, 301))[0]);
3304
3305 /* During 'AltitudeControl': '<S326>:359' */
3306 /* Transition: '<S326>:361' */
3307 if (_rtB->hcmd_j - _rtB->mh_j < -40.0F) {
3308 /* Transition: '<S326>:364' */
3309 _rtB->aplonL_f = 3;
3310
3311 /* Constant: '<S17>/Constant16' */
3312 _rtB->gamacmd_k = -5.0F;
3313 } else if (_rtB->hcmd_j - _rtB->mh_j > 40.0F) {
3314 /* Transition: '<S326>:363' */
3315 _rtB->aplonL_f = 3;
3316
3317 /* Constant: '<S17>/Constant15' */
3318 _rtB->gamacmd_k = 5.0F;
3319 } else {
3320 /* Transition: '<S326>:362' */
3321 _rtB->aplonL_f = 4;
3322 _rtB->gamacmd_k = 0.0F;
3323 }
3324
3325 /* During 'XYPositionControl': '<S326>:367' */
3326 if (((uint8_T *)ssGetDWork(S, 536))[0] == AFCS_MODEL_IN_GPSFailPhiControl) {
3327 /* During 'GPSFailPhiControl': '<S326>:523' */
3328 if (_rtB->GPSFail == 0) {
3329 /* Transition: '<S326>:521' */
3330 ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_YrwControl1;
3331
3332 /* Entry Internal 'YrwControl1': '<S326>:369' */
3333 /* Transition: '<S326>:370' */
3334 ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_phiLcontrol;
3335
3336 /* Entry 'phiLcontrol': '<S326>:372' */
3337 _rtB->aplatL_l = 5;
3338 _rtB->psdcmd_i = 0.0F;
3339 ((int32_T *)ssGetDWork(S, 305))[0] = 0;
3340 }
3341 } else {
3342 /* During 'YrwControl1': '<S326>:369' */
3343 if (_rtB->GPSFail == 1) {
3344 /* Transition: '<S326>:522' */
3345 /* Exit Internal 'YrwControl1': '<S326>:369' */
3346 ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3347 ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL_IN_GPSFailPhiControl;
3348
3349 /* Entry 'GPSFailPhiControl': '<S326>:523' */
3350 _rtB->aplatL_l = 2;
3351 _rtB->apyawL_m = 1;
3352 _rtB->phiLcmd_c = 30.0F;
3353 } else if (((uint8_T *)ssGetDWork(S, 538))[0] ==
3354 AFCS_MODEL1_IN_YrwControl_o) {
3355 /* During 'YrwControl': '<S326>:378' */
3356 /* Transition: '<S326>:380' */
3357 if (_rtB->yrw - _rtB->yrwcmd_o > 30.0F) {
3358 /* Transition: '<S326>:382' */
3359 _rtB->aplatL_l = 4;
3360 _rtB->psdcmd_i = -20.0F;
3361 _rtB->phiLcmd_c = 0.0F;
3362 } else if (_rtB->yrw - _rtB->yrwcmd_o < -30.0F) {
3363 /* Transition: '<S326>:383' */
3364 _rtB->aplatL_l = 4;
3365 _rtB->psdcmd_i = 20.0F;
3366 _rtB->phiLcmd_c = 0.0F;
3367 } else {
3368 /* Transition: '<S326>:381' */
3369 _rtB->aplatL_l = 5;
3370 _rtB->yrwcmd_o = 0.0F;
3371 _rtB->psdcmd_i = 0.0F;
3372 _rtB->phiLcmd_c = 0.0F;
3373 }
3374 } else {
3375 /* During 'phiLcontrol': '<S326>:372' */
3376 if ((((int32_T *)ssGetDWork(S, 305))[0] == 5) && (_rtB->SWAUTO == 1)) {
3377 /* Transition: '<S326>:371' */
3378 ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_YrwControl_o;
3379 } else {
3380 ((int32_T *)ssGetDWork(S, 305))[0] = AFCS_MODEL1_addup_jz((real32_T)
3381 fabs(AFCS_MODEL1_res180_m(_rtB->psdcmd_i - _rtB->psd)) < 15.0F, 5,
3382 ((int32_T *)ssGetDWork(S, 305))[0]);
3383
3384 /* Transition: '<S326>:374' */
3385 if (AFCS_MODEL1_res180_m(_rtB->psdcmd_i - _rtB->psd) > 20.0F) {
3386 /* Transition: '<S326>:375' */
3387 _rtB->aplatL_l = 2;
3388 _rtB->phiLcmd_c = 30.0F;
3389 } else {
3390 if (AFCS_MODEL1_res180_m(_rtB->psdcmd_i - _rtB->psd) < -20.0F) {
3391 /* Transition: '<S326>:377' */
3392 _rtB->aplatL_l = 2;
3393 _rtB->phiLcmd_c = -30.0F;
3394 } else {
3395 /* Transition: '<S326>:376' */
3396 _rtB->aplatL_l = 4;
3397 _rtB->psdcmd_i = 0.0F;
3398 _rtB->phiLcmd_c = 0.0F;
3399 }
3400 }
3401 }
3402 }
3403 }
3404 }
3405}
3406
3407/* Function for Chart: '<S316>/Landing2' */
3408static void AFCS_MODE_exit_internal_Landing(SimStruct *S)
3409{
3410 /* Exit Internal 'Landing': '<S326>:38' */
3411 ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3412
3413 /* Exit Internal 'GoAround': '<S326>:386' */
3414 ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3415
3416 /* Exit Internal 'FlightMode': '<S326>:454' */
3417 /* Exit Internal 'XYPositionControl': '<S326>:461' */
3418 /* Exit Internal 'EmYrwControl': '<S326>:480' */
3419 ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3420
3421 /* Exit Internal 'YrwControl11': '<S326>:548' */
3422 ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3423 ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3424
3425 /* Exit Internal 'NormYrwControl': '<S326>:465' */
3426 ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3427
3428 /* Exit Internal 'YrwControl1': '<S326>:527' */
3429 ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3430 ((uint8_T *)ssGetDWork(S, 542))[0] = 0U;
3431 ((uint8_T *)ssGetDWork(S, 540))[0] = 0U;
3432
3433 /* Exit Internal 'Pre_Land': '<S326>:357' */
3434 /* Exit Internal 'XYPositionControl': '<S326>:367' */
3435 ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3436
3437 /* Exit Internal 'YrwControl1': '<S326>:369' */
3438 ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3439 ((uint8_T *)ssGetDWork(S, 537))[0] = 0U;
3440 ((uint8_T *)ssGetDWork(S, 535))[0] = 0U;
3441}
3442
3443/* Function for Chart: '<S316>/Landing2' */
3444static void AFCS_MODEL1_XYPositionControl_d(SimStruct *S)
3445{
3446 int64_T tmp;
3447 B_AFCS_MODEL1_T *_rtB;
3448 _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));
3449
3450 /* During 'XYPositionControl': '<S326>:461' */
3451 if (((uint8_T *)ssGetDWork(S, 541))[0] == AFCS_MODEL1_IN_EmYrwControl) {
3452 /* During 'EmYrwControl': '<S326>:480' */
3453 if (((int8_T *)ssGetDWork(S, 511))[0] == 1) {
3454 /* Transition: '<S326>:463' */
3455 /* Exit Internal 'EmYrwControl': '<S326>:480' */
3456 ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3457
3458 /* Exit Internal 'YrwControl11': '<S326>:548' */
3459 ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3460 ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_NormYrwControl;
3461
3462 /* Entry 'NormYrwControl': '<S326>:465' */
3463 /* Entry Internal 'NormYrwControl': '<S326>:465' */
3464 /* Transition: '<S326>:547' */
3465 ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_YrwControl1;
3466
3467 /* Entry Internal 'YrwControl1': '<S326>:527' */
3468 /* Transition: '<S326>:545' */
3469 ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_phiLcontrol;
3470
3471 /* Entry 'phiLcontrol': '<S326>:533' */
3472 _rtB->aplatL_l = 4;
3473 _rtB->apyawL_m = 1;
3474 _rtB->psdcmd_i = 0.0F;
3475 ((int32_T *)ssGetDWork(S, 305))[0] = 0;
3476 } else {
3477 switch (((uint8_T *)ssGetDWork(S, 545))[0]) {
3478 case AFCS_MODEL_IN_GPSFailPhiControl:
3479 /* During 'GPSFailPhiControl': '<S326>:569' */
3480 if (_rtB->GPSFail == 0) {
3481 /* Transition: '<S326>:571' */
3482 ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_YrwControl11;
3483
3484 /* Entry Internal 'YrwControl11': '<S326>:548' */
3485 /* Transition: '<S326>:561' */
3486 ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_phiLcontrol2;
3487
3488 /* Entry 'phiLcontrol2': '<S326>:555' */
3489 _rtB->aplatL_l = 2;
3490 _rtB->apyawL_m = 1;
3491 ((int32_T *)ssGetDWork(S, 309))[0] = 0;
3492 ((int8_T *)ssGetDWork(S, 511))[0] = 0;
3493 } else {
3494 _rtB->aplatL_l = 2;
3495 _rtB->apyawL_m = 1;
3496 _rtB->phiLcmd_c = 30.0F;
3497 ((int32_T *)ssGetDWork(S, 310))[0] = 0;
3498 }
3499 break;
3500
3501 case AFCS_MODEL1_IN_PhiLontrol3:
3502 /* During 'PhiLontrol3': '<S326>:497' */
3503 if (((int32_T *)ssGetDWork(S, 312))[0] == 5) {
3504 /* Transition: '<S326>:484' */
3505 ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_TranseNorm_c;
3506
3507 /* Entry 'TranseNorm': '<S326>:498' */
3508 _rtB->StartEmNav_a = 0;
3509 ((int8_T *)ssGetDWork(S, 511))[0] = 1;
3510 tmp = ((int32_T *)ssGetDWork(S, 308))[0] + 1LL;
3511 if (tmp > 2147483647LL) {
3512 tmp = 2147483647LL;
3513 } else {
3514 if (tmp < -2147483648LL) {
3515 tmp = -2147483648LL;
3516 }
3517 }
3518
3519 ((int32_T *)ssGetDWork(S, 308))[0] = (int32_T)tmp;
3520 } else {
3521 _rtB->phiLcmd_c = 30.0F * _rtB->DireFg;
3522 ((int32_T *)ssGetDWork(S, 312))[0] = AFCS_MODEL1_addup_jz((real32_T)
3523 fabs(AFCS_MODEL1_res180_m(_rtB->DetaPsi)) < 15.0F, 5, ((int32_T *)
3524 ssGetDWork(S, 312))[0]);
3525 }
3526 break;
3527
3528 case AFCS_MODEL1_IN_TranseNorm_c:
3529 /* During 'TranseNorm': '<S326>:498' */
3530 break;
3531
3532 default:
3533 /* During 'YrwControl11': '<S326>:548' */
3534 if (_rtB->GPSFail == 1) {
3535 /* Transition: '<S326>:570' */
3536 /* Exit Internal 'YrwControl11': '<S326>:548' */
3537 ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3538 ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL_IN_GPSFailPhiControl;
3539 } else if (((int32_T *)ssGetDWork(S, 310))[0] == 5) {
3540 /* Transition: '<S326>:483' */
3541 /* Exit Internal 'YrwControl11': '<S326>:548' */
3542 ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3543 ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_PhiLontrol3;
3544
3545 /* Entry 'PhiLontrol3': '<S326>:497' */
3546 _rtB->aplatL_l = 2;
3547 _rtB->apyawL_m = 1;
3548 ((int32_T *)ssGetDWork(S, 312))[0] = 0;
3549 } else if (((uint8_T *)ssGetDWork(S, 546))[0] ==
3550 AFCS_MODEL1_IN_YrwControl_o) {
3551 /* During 'YrwControl': '<S326>:563' */
3552 ((int32_T *)ssGetDWork(S, 310))[0] = AFCS_MODEL1_addup_jz
3553 ((_rtB->drange < 50.0F) && (_rtB->GPSFail == 0), 5, ((int32_T *)
3554 ssGetDWork(S, 310))[0]);
3555
3556 /* Transition: '<S326>:565' */
3557 if (_rtB->yrw - _rtB->yrwcmd_o > 50.0F) {
3558 /* Transition: '<S326>:567' */
3559 _rtB->aplatL_l = 4;
3560 _rtB->psdcmd_i = -10.0F;
3561 _rtB->phiLcmd_c = 0.0F;
3562 } else if (_rtB->yrw - _rtB->yrwcmd_o < -50.0F) {
3563 /* Transition: '<S326>:568' */
3564 _rtB->aplatL_l = 4;
3565 _rtB->psdcmd_i = 10.0F;
3566 _rtB->phiLcmd_c = 0.0F;
3567 } else {
3568 /* Transition: '<S326>:566' */
3569 _rtB->aplatL_l = 5;
3570 _rtB->yrwcmd_o = 0.0F;
3571 _rtB->psdcmd_i = 0.0F;
3572 _rtB->phiLcmd_c = 0.0F;
3573 }
3574 } else {
3575 /* During 'phiLcontrol2': '<S326>:555' */
3576 if ((((int32_T *)ssGetDWork(S, 309))[0] == 5) && (_rtB->SWAUTO == 1))
3577 {
3578 /* Transition: '<S326>:562' */
3579 _rtB->StartEmNav_a = 1;
3580 ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_YrwControl_o;
3581
3582 /* Entry 'YrwControl': '<S326>:563' */
3583 ((int32_T *)ssGetDWork(S, 310))[0] = 0;
3584 } else {
3585 ((int32_T *)ssGetDWork(S, 309))[0] = AFCS_MODEL1_addup_jz((real32_T)
3586 fabs(AFCS_MODEL1_res180_m(_rtB->DetaPsi)) < 15.0F, 5, ((int32_T *)
3587 ssGetDWork(S, 309))[0]);
3588
3589 /* Transition: '<S326>:557' */
3590 if (AFCS_MODEL1_res180_m(_rtB->DetaPsi) > 15.0F) {
3591 /* Transition: '<S326>:558' */
3592 _rtB->aplatL_l = 2;
3593 _rtB->phiLcmd_c = 30.0F;
3594 } else {
3595 if (AFCS_MODEL1_res180_m(_rtB->DetaPsi) < -15.0F) {
3596 /* Transition: '<S326>:560' */
3597 _rtB->aplatL_l = 2;
3598 _rtB->phiLcmd_c = -30.0F;
3599 } else {
3600 /* Transition: '<S326>:559' */
3601 _rtB->aplatL_l = 2;
3602 _rtB->phiLcmd_c = 0.0F;
3603 }
3604 }
3605 }
3606 }
3607 break;
3608 }
3609 }
3610 } else {
3611 /* During 'NormYrwControl': '<S326>:465' */
3612 if ((_rtB->GoaroundFg == 1) && (((int32_T *)ssGetDWork(S, 308))[0] == 0)) {
3613 /* Transition: '<S326>:464' */
3614 /* Exit Internal 'NormYrwControl': '<S326>:465' */
3615 ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3616
3617 /* Exit Internal 'YrwControl1': '<S326>:527' */
3618 ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3619 ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_EmYrwControl;
3620
3621 /* Entry Internal 'EmYrwControl': '<S326>:480' */
3622 /* Transition: '<S326>:481' */
3623 ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_YrwControl11;
3624
3625 /* Entry Internal 'YrwControl11': '<S326>:548' */
3626 /* Transition: '<S326>:561' */
3627 ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_phiLcontrol2;
3628
3629 /* Entry 'phiLcontrol2': '<S326>:555' */
3630 _rtB->aplatL_l = 2;
3631 _rtB->apyawL_m = 1;
3632 ((int32_T *)ssGetDWork(S, 309))[0] = 0;
3633 ((int8_T *)ssGetDWork(S, 511))[0] = 0;
3634 } else if (((uint8_T *)ssGetDWork(S, 543))[0] ==
3635 AFCS_MODEL_IN_GPSFailPhiControl) {
3636 /* During 'GPSFailPhiControl': '<S326>:526' */
3637 if (_rtB->GPSFail == 0) {
3638 /* Transition: '<S326>:524' */
3639 ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_YrwControl1;
3640
3641 /* Entry Internal 'YrwControl1': '<S326>:527' */
3642 /* Transition: '<S326>:545' */
3643 ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_phiLcontrol;
3644
3645 /* Entry 'phiLcontrol': '<S326>:533' */
3646 _rtB->aplatL_l = 4;
3647 _rtB->apyawL_m = 1;
3648 _rtB->psdcmd_i = 0.0F;
3649 ((int32_T *)ssGetDWork(S, 305))[0] = 0;
3650 }
3651 } else {
3652 /* During 'YrwControl1': '<S326>:527' */
3653 if (_rtB->GPSFail == 1) {
3654 /* Transition: '<S326>:525' */
3655 /* Exit Internal 'YrwControl1': '<S326>:527' */
3656 ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3657 ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL_IN_GPSFailPhiControl;
3658
3659 /* Entry 'GPSFailPhiControl': '<S326>:526' */
3660 _rtB->aplatL_l = 2;
3661 _rtB->apyawL_m = 1;
3662 _rtB->phiLcmd_c = 30.0F;
3663 } else if (((uint8_T *)ssGetDWork(S, 544))[0] ==
3664 AFCS_MODEL1_IN_YrwControl_o) {
3665 /* During 'YrwControl': '<S326>:539' */
3666 /* Transition: '<S326>:541' */
3667 if (_rtB->yrw - _rtB->yrwcmd_o > 50.0F) {
3668 /* Transition: '<S326>:543' */
3669 _rtB->aplatL_l = 4;
3670 _rtB->psdcmd_i = -10.0F;
3671 _rtB->phiLcmd_c = 0.0F;
3672 } else if (_rtB->yrw - _rtB->yrwcmd_o < -50.0F) {
3673 /* Transition: '<S326>:544' */
3674 _rtB->aplatL_l = 4;
3675 _rtB->psdcmd_i = 10.0F;
3676 _rtB->phiLcmd_c = 0.0F;
3677 } else {
3678 /* Transition: '<S326>:542' */
3679 _rtB->aplatL_l = 5;
3680 _rtB->yrwcmd_o = 0.0F;
3681 _rtB->psdcmd_i = 0.0F;
3682 _rtB->phiLcmd_c = 0.0F;
3683 }
3684 } else {
3685 /* During 'phiLcontrol': '<S326>:533' */
3686 if ((((int32_T *)ssGetDWork(S, 305))[0] == 5) && (_rtB->SWAUTO == 1) &&
3687 (_rtB->GPSFail == 0)) {
3688 /* Transition: '<S326>:546' */
3689 ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_YrwControl_o;
3690 } else {
3691 ((int32_T *)ssGetDWork(S, 305))[0] = AFCS_MODEL1_addup_jz((real32_T)
3692 fabs(AFCS_MODEL1_res180_m(_rtB->psdcmd_i - _rtB->psd)) < 15.0F, 5,
3693 ((int32_T *)ssGetDWork(S, 305))[0]);
3694
3695 /* Transition: '<S326>:535' */
3696 if (AFCS_MODEL1_res180_m(_rtB->psdcmd_i - _rtB->psd) > 20.0F) {
3697 /* Transition: '<S326>:536' */
3698 _rtB->aplatL_l = 2;
3699 _rtB->phiLcmd_c = 30.0F;
3700 } else {
3701 if (AFCS_MODEL1_res180_m(_rtB->psdcmd_i - _rtB->psd) < -20.0F) {
3702 /* Transition: '<S326>:538' */
3703 _rtB->aplatL_l = 2;
3704 _rtB->phiLcmd_c = -30.0F;
3705 } else {
3706 /* Transition: '<S326>:537' */
3707 _rtB->aplatL_l = 4;
3708 _rtB->psdcmd_i = 0.0F;
3709 _rtB->phiLcmd_c = 0.0F;
3710 }
3711 }
3712 }
3713 }
3714 }
3715 }
3716}
3717
3718/* Function for Chart: '<S316>/Landing2' */
3719static void AFCS_MODEL1_Landing(SimStruct *S)
3720{
3721 int32_T tmp;
3722 B_AFCS_MODEL1_T *_rtB;
3723 _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));
3724
3725 /* Constant: '<S17>/Constant11' */
3726 /* During 'Landing': '<S326>:38' */
3727 ((real32_T *)ssGetDWork(S, 286))[0] = _rtB->xrw * 0.0349181965F;
3728 if (_rtB->Switch6_j == 1) {
3729 /* Transition: '<S326>:520' */
3730 _rtB->fmLandtoTaxi = 1;
3731 tmp = _rtB->K_k + 1;
3732 if (tmp > 127) {
3733 tmp = 127;
3734 }
3735
3736 _rtB->K_k = (int8_T)tmp;
3737 AFCS_MODE_exit_internal_Landing(S);
3738
3739 /* Entry Internal 'Landing': '<S326>:38' */
3740 /* Transition: '<S326>:347' */
3741 ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_Pre_Land;
3742
3743 /* Entry 'Pre_Land': '<S326>:357' */
3744 ((int32_T *)ssGetDWork(S, 301))[0] = 0;
3745
3746 /* Entry Internal 'Pre_Land': '<S326>:357' */
3747 ((uint8_T *)ssGetDWork(S, 535))[0] = 1U;
3748 ((uint8_T *)ssGetDWork(S, 537))[0] = 1U;
3749
3750 /* Entry Internal 'XYPositionControl': '<S326>:367' */
3751 /* Transition: '<S326>:368' */
3752 ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_YrwControl1;
3753
3754 /* Entry Internal 'YrwControl1': '<S326>:369' */
3755 /* Transition: '<S326>:370' */
3756 ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_phiLcontrol;
3757
3758 /* Entry 'phiLcontrol': '<S326>:372' */
3759 _rtB->aplatL_l = 5;
3760 _rtB->psdcmd_i = 0.0F;
3761 ((int32_T *)ssGetDWork(S, 305))[0] = 0;
3762 } else {
3763 switch (((uint8_T *)ssGetDWork(S, 534))[0]) {
3764 case AFCS_MODEL1_IN_Decline:
3765 /* During 'Decline': '<S326>:352' */
3766 if (((int32_T *)ssGetDWork(S, 302))[0] == 5) {
3767 /* Transition: '<S326>:354' */
3768 ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_GamaControl1;
3769
3770 /* Entry 'GamaControl1': '<S326>:202' */
3771 _rtB->aplonL_f = 3;
3772 _rtB->gamacmd_k = -1.5F;
3773 _rtB->apthL_e = 3;
3774 _rtB->thrustcmd_ho = 0.0F;
3775 _rtB->vtcmd_f = 30.0F;
3776 _rtB->aplatL_l = 5;
3777 ((int32_T *)ssGetDWork(S, 303))[0] = 0;
3778 } else if ((_rtB->GoaroundFg == 1) && (_rtB->mh_j > 30.0F) && (((int32_T *)
3779 ssGetDWork(S, 308))[0] == 0)) {
3780 /* Transition: '<S326>:387' */
3781 ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_GoAround;
3782
3783 /* Entry Internal 'GoAround': '<S326>:386' */
3784 /* Transition: '<S326>:388' */
3785 ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_Climb;
3786
3787 /* Entry 'Climb': '<S326>:390' */
3788 _rtB->apthL_e = 3;
3789 _rtB->aplonL_f = 4;
3790 _rtB->aplatL_l = 2;
3791 _rtB->apyawL_m = 1;
3792 ((int32_T *)ssGetDWork(S, 307))[0] = 0;
3793 } else if ((_rtB->GPSFail == 1) && (_rtB->mh_j > 15.0F)) {
3794 /* Transition: '<S326>:575' */
3795 ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_Pre_Land;
3796
3797 /* Entry 'Pre_Land': '<S326>:357' */
3798 ((int32_T *)ssGetDWork(S, 301))[0] = 0;
3799
3800 /* Entry Internal 'Pre_Land': '<S326>:357' */
3801 ((uint8_T *)ssGetDWork(S, 535))[0] = 1U;
3802 ((uint8_T *)ssGetDWork(S, 537))[0] = 1U;
3803
3804 /* Entry Internal 'XYPositionControl': '<S326>:367' */
3805 /* Transition: '<S326>:368' */
3806 ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_YrwControl1;
3807
3808 /* Entry Internal 'YrwControl1': '<S326>:369' */
3809 /* Transition: '<S326>:370' */
3810 ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_phiLcontrol;
3811
3812 /* Entry 'phiLcontrol': '<S326>:372' */
3813 _rtB->aplatL_l = 5;
3814 _rtB->psdcmd_i = 0.0F;
3815 ((int32_T *)ssGetDWork(S, 305))[0] = 0;
3816 } else {
3817 _rtB->aplonL_f = 4;
3818 _rtB->hcmd_j = ((real32_T *)ssGetDWork(S, 286))[0];
3819 _rtB->apsbL_l = 2;
3820 _rtB->aplatL_l = 5;
3821 _rtB->psdcmd_i = 0.0F;
3822 _rtB->phiLcmd_c = 0.0F;
3823 _rtB->apnwsL_m = 2;
3824
3825 /* Constant: '<S17>/Constant11' */
3826 _rtB->gamacmd_k = -2.0F;
3827 _rtB->apthL_e = 3;
3828
3829 /* Constant: '<S17>/Constant4' */
3830 _rtB->vtcmd_f = 35.0F;
3831 ((int32_T *)ssGetDWork(S, 302))[0] = AFCS_MODEL1_addup_jz(_rtB->mh_j <
3832 10.0F, 5, ((int32_T *)ssGetDWork(S, 302))[0]);
3833 if ((_rtB->GPSFail == 1) && (_rtB->mh_j < 15.0F)) {
3834 /* Transition: '<S326>:574' */
3835 _rtB->aplatL_l = 1;
3836 _rtB->phiLcmd_c = 0.0F;
3837 }
3838 }
3839 break;
3840
3841 case AFCS_MODEL1_IN_GamaControl1:
3842 /* During 'GamaControl1': '<S326>:202' */
3843 if (((int32_T *)ssGetDWork(S, 303))[0] == 5) {
3844 /* Transition: '<S326>:205' */
3845 ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_GamaControl2;
3846
3847 /* Entry 'GamaControl2': '<S326>:204' */
3848 _rtB->aplatL_l = 2;
3849 _rtB->phiLcmd_c = 0.0F;
3850 _rtB->gamacmd_k = -0.5F;
3851 _rtB->apyawL_m = 3;
3852 _rtB->apthL_e = 3;
3853 _rtB->vtcmd_f = 30.0F;
3854 _rtB->thrustcmd_ho = 0.0F;
3855 ((int32_T *)ssGetDWork(S, 304))[0] = 0;
3856 } else {
3857 ((int32_T *)ssGetDWork(S, 303))[0] = AFCS_MODEL1_addup_jz(_rtB->mh_j <
3858 4.0F, 5, ((int32_T *)ssGetDWork(S, 303))[0]);
3859 }
3860 break;
3861
3862 case AFCS_MODEL1_IN_GamaControl2:
3863 /* During 'GamaControl2': '<S326>:204' */
3864 if (((int32_T *)ssGetDWork(S, 304))[0] == 5) {
3865 /* Transition: '<S326>:207' */
3866 ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_VupControl3;
3867
3868 /* Entry 'VupControl3': '<S326>:206' */
3869 _rtB->fmLandtoTaxi = 1;
3870 } else {
3871 ((int32_T *)ssGetDWork(S, 304))[0] = AFCS_MODEL1_addup_jz((_rtB->mh_j <
3872 1.0F) && (_rtB->Switch6_j == 1), 5, ((int32_T *)ssGetDWork(S, 304))[0]);
3873 }
3874 break;
3875
3876 case AFCS_MODEL1_IN_GoAround:
3877 /* During 'GoAround': '<S326>:386' */
3878 if (((int8_T *)ssGetDWork(S, 512))[0] == 1) {
3879 /* Transition: '<S326>:501' */
3880 /* Exit Internal 'GoAround': '<S326>:386' */
3881 ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3882
3883 /* Exit Internal 'FlightMode': '<S326>:454' */
3884 /* Exit Internal 'XYPositionControl': '<S326>:461' */
3885 /* Exit Internal 'EmYrwControl': '<S326>:480' */
3886 ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3887
3888 /* Exit Internal 'YrwControl11': '<S326>:548' */
3889 ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3890 ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3891
3892 /* Exit Internal 'NormYrwControl': '<S326>:465' */
3893 ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3894
3895 /* Exit Internal 'YrwControl1': '<S326>:527' */
3896 ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3897 ((uint8_T *)ssGetDWork(S, 542))[0] = 0U;
3898 ((uint8_T *)ssGetDWork(S, 540))[0] = 0U;
3899 ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_Pre_Land;
3900
3901 /* Entry 'Pre_Land': '<S326>:357' */
3902 ((int32_T *)ssGetDWork(S, 301))[0] = 0;
3903
3904 /* Entry Internal 'Pre_Land': '<S326>:357' */
3905 ((uint8_T *)ssGetDWork(S, 535))[0] = 1U;
3906 ((uint8_T *)ssGetDWork(S, 537))[0] = 1U;
3907
3908 /* Entry Internal 'XYPositionControl': '<S326>:367' */
3909 /* Transition: '<S326>:368' */
3910 ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_YrwControl1;
3911
3912 /* Entry Internal 'YrwControl1': '<S326>:369' */
3913 /* Transition: '<S326>:370' */
3914 ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_phiLcontrol;
3915
3916 /* Entry 'phiLcontrol': '<S326>:372' */
3917 _rtB->aplatL_l = 5;
3918 _rtB->psdcmd_i = 0.0F;
3919 ((int32_T *)ssGetDWork(S, 305))[0] = 0;
3920 } else {
3921 switch (((uint8_T *)ssGetDWork(S, 539))[0]) {
3922 case AFCS_MODEL1_IN_Climb:
3923 /* During 'Climb': '<S326>:390' */
3924 if (((int32_T *)ssGetDWork(S, 307))[0] == 5) {
3925 /* Transition: '<S326>:399' */
3926 ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_LeftSpiral10s;
3927
3928 /* Entry 'LeftSpiral10s': '<S326>:398' */
3929 _rtB->apthL_e = 3;
3930 _rtB->aplonL_f = 4;
3931 _rtB->aplatL_l = 2;
3932 _rtB->apyawL_m = 1;
3933 ((int32_T *)ssGetDWork(S, 306))[0] = 0;
3934 } else {
3935 _rtB->hcmd_j = _rtB->HrefAL;
3936
3937 /* Constant: '<S17>/Constant6' */
3938 _rtB->vtcmd_f = 40.0F;
3939 _rtB->phiLcmd_c = 0.0F;
3940 ((int32_T *)ssGetDWork(S, 307))[0] = AFCS_MODEL1_addup_jz(_rtB->mh_j
3941 > 30.0F, 5, ((int32_T *)ssGetDWork(S, 307))[0]);
3942
3943 /* Transition: '<S326>:392' */
3944 if (_rtB->hcmd_j - _rtB->mh_j < -40.0F) {
3945 /* Transition: '<S326>:394' */
3946 _rtB->aplonL_f = 3;
3947
3948 /* Constant: '<S17>/Constant16' */
3949 _rtB->gamacmd_k = -5.0F;
3950 } else if (_rtB->hcmd_j - _rtB->mh_j > 40.0F) {
3951 /* Transition: '<S326>:396' */
3952 _rtB->aplonL_f = 3;
3953
3954 /* Constant: '<S17>/Constant15' */
3955 _rtB->gamacmd_k = 5.0F;
3956 } else {
3957 /* Transition: '<S326>:393' */
3958 _rtB->aplonL_f = 4;
3959 _rtB->gamacmd_k = 0.0F;
3960 }
3961 }
3962 break;
3963
3964 case AFCS_MODEL1_IN_FlightMode_e:
3965 /* During 'FlightMode': '<S326>:454' */
3966 if (((int32_T *)ssGetDWork(S, 311))[0] == 5) {
3967 /* Transition: '<S326>:500' */
3968 /* Exit Internal 'FlightMode': '<S326>:454' */
3969 /* Exit Internal 'XYPositionControl': '<S326>:461' */
3970 /* Exit Internal 'EmYrwControl': '<S326>:480' */
3971 ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3972
3973 /* Exit Internal 'YrwControl11': '<S326>:548' */
3974 ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3975 ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3976
3977 /* Exit Internal 'NormYrwControl': '<S326>:465' */
3978 ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3979
3980 /* Exit Internal 'YrwControl1': '<S326>:527' */
3981 ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
3982 ((uint8_T *)ssGetDWork(S, 542))[0] = 0U;
3983 ((uint8_T *)ssGetDWork(S, 540))[0] = 0U;
3984 ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_TransLand_o;
3985
3986 /* Entry 'TransLand': '<S326>:499' */
3987 ((int8_T *)ssGetDWork(S, 512))[0] = 1;
3988 _rtB->K_k = _rtB->IndexAL;
3989 } else {
3990 /* Constant: '<S17>/Constant10' */
3991 ((int32_T *)ssGetDWork(S, 311))[0] = AFCS_MODEL1_addup_jz
3992 ((_rtB->drange < 400.0F) && ((real32_T)fabs(_rtB->href -
3993 _rtB->mh_j) < 50.0F) && ((real32_T)fabs(_rtB->yrw -
3994 _rtB->yrwcmd_o) < 50.0F) && (_rtB->SWAUTO == 1) &&
3995 (_rtB->StartEmNav_a == 0), 5, ((int32_T *)ssGetDWork(S, 311))[0]);
3996
3997 /* During 'AltitudeControl': '<S326>:455' */
3998 /* Transition: '<S326>:457' */
3999 if (_rtB->hcmd_j - _rtB->mh_j < -20.0F) {
4000 /* Transition: '<S326>:460' */
4001 _rtB->aplonL_f = 3;
4002
4003 /* Constant: '<S17>/Constant16' */
4004 _rtB->gamacmd_k = -5.0F;
4005 } else if (_rtB->hcmd_j - _rtB->mh_j > 20.0F) {
4006 /* Transition: '<S326>:459' */
4007 _rtB->aplonL_f = 3;
4008
4009 /* Constant: '<S17>/Constant15' */
4010 _rtB->gamacmd_k = 5.0F;
4011 } else {
4012 /* Transition: '<S326>:458' */
4013 _rtB->aplonL_f = 4;
4014 _rtB->gamacmd_k = 0.0F;
4015 }
4016
4017 AFCS_MODEL1_XYPositionControl_d(S);
4018 }
4019 break;
4020
4021 case AFCS_MODEL1_IN_LeftSpiral10s:
4022 /* During 'LeftSpiral10s': '<S326>:398' */
4023 if (((int32_T *)ssGetDWork(S, 306))[0] == 2500) {
4024 /* Transition: '<S326>:401' */
4025 ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_FlightMode_e;
4026
4027 /* Entry 'FlightMode': '<S326>:454' */
4028 ((int32_T *)ssGetDWork(S, 311))[0] = 0;
4029 tmp = _rtB->IndexAL - 1;
4030 if (tmp < -128) {
4031 tmp = -128;
4032 }
4033
4034 _rtB->K_k = (int8_T)tmp;
4035
4036 /* Entry Internal 'FlightMode': '<S326>:454' */
4037 ((uint8_T *)ssGetDWork(S, 540))[0] = 1U;
4038 ((uint8_T *)ssGetDWork(S, 542))[0] = 1U;
4039
4040 /* Entry Internal 'XYPositionControl': '<S326>:461' */
4041 /* Transition: '<S326>:462' */
4042 ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_NormYrwControl;
4043
4044 /* Entry 'NormYrwControl': '<S326>:465' */
4045 /* Entry Internal 'NormYrwControl': '<S326>:465' */
4046 /* Transition: '<S326>:547' */
4047 ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_YrwControl1;
4048
4049 /* Entry Internal 'YrwControl1': '<S326>:527' */
4050 /* Transition: '<S326>:545' */
4051 ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_phiLcontrol;
4052
4053 /* Entry 'phiLcontrol': '<S326>:533' */
4054 _rtB->aplatL_l = 4;
4055 _rtB->apyawL_m = 1;
4056 _rtB->psdcmd_i = 0.0F;
4057 ((int32_T *)ssGetDWork(S, 305))[0] = 0;
4058 } else {
4059 _rtB->hcmd_j = _rtB->HrefAL;
4060
4061 /* Constant: '<S17>/Constant6' */
4062 _rtB->vtcmd_f = 40.0F;
4063 _rtB->phiLcmd_c = -30.0F;
4064 ((int32_T *)ssGetDWork(S, 306))[0] = AFCS_MODEL1_addup_jz(_rtB->mh_j
4065 > _rtB->HrefAL - 20.0F, 2500, ((int32_T *)ssGetDWork(S, 306))[0]);
4066 }
4067 break;
4068
4069 default:
4070 /* During 'TransLand': '<S326>:499' */
4071 break;
4072 }
4073 }
4074 break;
4075
4076 case AFCS_MODEL1_IN_Pre_Land:
4077 AFCS_MODEL1_Pre_Land(S);
4078 break;
4079
4080 default:
4081 /* During 'VupControl3': '<S326>:206' */
4082 break;
4083 }
4084 }
4085}
4086
4087/* Function for Chart: '<S322>/Taxi' */
4088static int32_T AFCS_MODEL1_addup_od(boolean_T bA, int32_T iB, int32_T iC)
4089{
4090 int32_T tC;
4091 int64_T tmp;
4092
4093 /* MATLAB Function 'addup': '<S330>:121' */
4094 /* Graphical Function 'addup': '<S330>:121' */
4095 /* '<S330>:129:1' */
4096 tC = iC;
4097
4098 /* '<S330>:130:1' */
4099 if (!bA) {
4100 /* '<S330>:133:1' */
4101 if (iC > 0) {
4102 /* '<S330>:133:1' */
4103 tC = iC - 1;
4104 }
4105 } else {
4106 /* '<S330>:135:1' */
4107 if (iC > iB) {
4108 /* '<S330>:135:1' */
4109 tC = iB;
4110 } else {
4111 /* '<S330>:134:1' */
4112 tmp = iC + 1LL;
4113 if (tmp > 2147483647LL) {
4114 tmp = 2147483647LL;
4115 } else {
4116 if (tmp < -2147483648LL) {
4117 tmp = -2147483648LL;
4118 }
4119 }
4120
4121 tC = (int32_T)tmp;
4122 }
4123 }
4124
4125 return tC;
4126}
4127
4128/* Function for Chart: '<S319>/Pre_Takeoff' */
4129static int32_T AFCS_MODEL1_addup_ke(int8_T bA, int32_T iB, int32_T iC)
4130{
4131 int32_T tC;
4132 int64_T tmp;
4133
4134 /* MATLAB Function 'addup': '<S327>:8' */
4135 /* Graphical Function 'addup': '<S327>:8' */
4136 /* '<S327>:13:1' */
4137 tC = iC;
4138
4139 /* '<S327>:16:1' */
4140 if (bA == 0) {
4141 /* '<S327>:18:1' */
4142 if (iC > 0) {
4143 /* '<S327>:18:1' */
4144 tC = iC - 1;
4145 }
4146 } else {
4147 /* '<S327>:24:1' */
4148 if (iC > iB) {
4149 /* '<S327>:24:1' */
4150 tC = iB;
4151 } else {
4152 /* '<S327>:26:1' */
4153 tmp = iC + 1LL;
4154 if (tmp > 2147483647LL) {
4155 tmp = 2147483647LL;
4156 } else {
4157 if (tmp < -2147483648LL) {
4158 tmp = -2147483648LL;
4159 }
4160 }
4161
4162 tC = (int32_T)tmp;
4163 }
4164 }
4165
4166 return tC;
4167}
4168
4169/* Function for Chart: '<S314>/Climb2' */
4170static int32_T AFCS_MODEL1_addup_j(boolean_T bA, int32_T iB, int32_T iC)
4171{
4172 int32_T tC;
4173 int64_T tmp;
4174
4175 /* MATLAB Function 'addup': '<S324>:154' */
4176 /* Graphical Function 'addup': '<S324>:154' */
4177 /* '<S324>:162:1' */
4178 tC = iC;
4179
4180 /* '<S324>:163:1' */
4181 if (!bA) {
4182 /* '<S324>:166:1' */
4183 if (iC > 0) {
4184 /* '<S324>:166:1' */
4185 tC = iC - 1;
4186 }
4187 } else {
4188 /* '<S324>:168:1' */
4189 if (iC > iB) {
4190 /* '<S324>:168:1' */
4191 tC = iB;
4192 } else {
4193 /* '<S324>:167:1' */
4194 tmp = iC + 1LL;
4195 if (tmp > 2147483647LL) {
4196 tmp = 2147483647LL;
4197 } else {
4198 if (tmp < -2147483648LL) {
4199 tmp = -2147483648LL;
4200 }
4201 }
4202
4203 tC = (int32_T)tmp;
4204 }
4205 }
4206
4207 return tC;
4208}
4209
4210/* Function for Chart: '<S313>/Approach2' */
4211static int32_T AFCS_MODEL1_addup_f(boolean_T bA, int32_T iB, int32_T iC)
4212{
4213 int32_T tC;
4214 int64_T tmp;
4215
4216 /* MATLAB Function 'addup': '<S323>:413' */
4217 /* Graphical Function 'addup': '<S323>:413' */
4218 /* '<S323>:421:1' */
4219 tC = iC;
4220
4221 /* '<S323>:422:1' */
4222 if (!bA) {
4223 /* '<S323>:425:1' */
4224 if (iC > 0) {
4225 /* '<S323>:425:1' */
4226 tC = iC - 1;
4227 }
4228 } else {
4229 /* '<S323>:427:1' */
4230 if (iC > iB) {
4231 /* '<S323>:427:1' */
4232 tC = iB;
4233 } else {
4234 /* '<S323>:426:1' */
4235 tmp = iC + 1LL;
4236 if (tmp > 2147483647LL) {
4237 tmp = 2147483647LL;
4238 } else {
4239 if (tmp < -2147483648LL) {
4240 tmp = -2147483648LL;
4241 }
4242 }
4243
4244 tC = (int32_T)tmp;
4245 }
4246 }
4247
4248 return tC;
4249}
4250
4251/* Function for Chart: '<S313>/Approach2' */
4252static real32_T AFCS_MODEL1_res180_l(real32_T angle)
4253{
4254 real32_T outangle;
4255
4256 /* MATLAB Function 'res180': '<S323>:175' */
4257 /* Graphical Function 'res180': '<S323>:175' */
4258 /* '<S323>:184:1' */
4259 if (angle > 180.0F) {
4260 /* '<S323>:184:1' */
4261 outangle = angle - 360.0F;
4262 } else {
4263 /* '<S323>:186:1' */
4264 if (angle < -180.0F) {
4265 /* '<S323>:186:1' */
4266 outangle = angle + 360.0F;
4267 } else {
4268 /* '<S323>:191:1' */
4269 outangle = angle;
4270 }
4271 }
4272
4273 return outangle;
4274}
4275
4276/* Function for Chart: '<S313>/Approach2' */
4277static void AFCS_MODEL1_XYPositionControl(SimStruct *S)
4278{
4279 int64_T tmp;
4280 B_AFCS_MODEL1_T *_rtB;
4281 _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));
4282
4283 /* During 'XYPositionControl': '<S323>:452' */
4284 if (((uint8_T *)ssGetDWork(S, 551))[0] == AFCS_MODEL1_IN_EmYrwControl) {
4285 /* During 'EmYrwControl': '<S323>:493' */
4286 if (((int8_T *)ssGetDWork(S, 513))[0] == 1) {
4287 /* Transition: '<S323>:494' */
4288 /* Exit Internal 'EmYrwControl': '<S323>:493' */
4289 ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
4290
4291 /* Exit Internal 'LevelPosition': '<S323>:567' */
4292 ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
4293 ((uint8_T *)ssGetDWork(S, 551))[0] = AFCS_MODEL1_IN_NormYrwControl;
4294
4295 /* Entry 'NormYrwControl': '<S323>:454' */
4296 /* Entry Internal 'NormYrwControl': '<S323>:454' */
4297 /* Transition: '<S323>:455' */
4298 ((uint8_T *)ssGetDWork(S, 553))[0] = AFCS_MODEL1_IN_YrwControl1;
4299
4300 /* Entry Internal 'YrwControl1': '<S323>:550' */
4301 /* Transition: '<S323>:551' */
4302 ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_phiLcontrol;
4303
4304 /* Entry 'phiLcontrol': '<S323>:554' */
4305 _rtB->aplatL_a = 5;
4306 _rtB->apyawL_fh = 1;
4307 _rtB->psdcmd_g = 0.0F;
4308 ((int32_T *)ssGetDWork(S, 314))[0] = 0;
4309 } else {
4310 switch (((uint8_T *)ssGetDWork(S, 555))[0]) {
4311 case AFCS_MODEL_IN_GPSFailPhiControl:
4312 /* During 'GPSFailPhiControl': '<S323>:583' */
4313 if (_rtB->GPSFail == 0) {
4314 /* Transition: '<S323>:589' */
4315 ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL1_IN_LevelPosition;
4316
4317 /* Entry Internal 'LevelPosition': '<S323>:567' */
4318 /* Transition: '<S323>:568' */
4319 ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_phiLcontrol2;
4320
4321 /* Entry 'phiLcontrol2': '<S323>:571' */
4322 _rtB->aplatL_a = 2;
4323 _rtB->apyawL_fh = 1;
4324 ((int32_T *)ssGetDWork(S, 315))[0] = 0;
4325 ((int8_T *)ssGetDWork(S, 513))[0] = 0;
4326 }
4327 break;
4328
4329 case AFCS_MODEL1_IN_LevelPosition:
4330 /* During 'LevelPosition': '<S323>:567' */
4331 if (_rtB->GPSFail == 1) {
4332 /* Transition: '<S323>:588' */
4333 /* Exit Internal 'LevelPosition': '<S323>:567' */
4334 ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
4335 ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL_IN_GPSFailPhiControl;
4336
4337 /* Entry 'GPSFailPhiControl': '<S323>:583' */
4338 _rtB->aplatL_a = 2;
4339 _rtB->apyawL_fh = 1;
4340 _rtB->phiLcmd_l4 = 30.0F;
4341 } else if (((int32_T *)ssGetDWork(S, 316))[0] == 5) {
4342 /* Transition: '<S323>:511' */
4343 /* Exit Internal 'LevelPosition': '<S323>:567' */
4344 ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
4345 ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL1_IN_PhiLControl3;
4346
4347 /* Entry 'PhiLControl3': '<S323>:527' */
4348 _rtB->aplatL_a = 2;
4349 _rtB->apyawL_fh = 1;
4350 ((int32_T *)ssGetDWork(S, 318))[0] = 0;
4351 } else if (((uint8_T *)ssGetDWork(S, 556))[0] ==
4352 AFCS_MODEL1_IN_YrwControl_o) {
4353 /* During 'YrwControl': '<S323>:577' */
4354 ((int32_T *)ssGetDWork(S, 316))[0] = AFCS_MODEL1_addup_f((_rtB->drange
4355 < 100.0F) && (_rtB->GPSFail == 0), 5, ((int32_T *)ssGetDWork(S, 316))
4356 [0]);
4357
4358 /* Transition: '<S323>:579' */
4359 if (_rtB->yrw - _rtB->yrwcmd_i > 50.0F) {
4360 /* Transition: '<S323>:581' */
4361 _rtB->aplatL_a = 4;
4362 _rtB->psdcmd_g = -10.0F;
4363 _rtB->phiLcmd_l4 = 0.0F;
4364 } else if (_rtB->yrw - _rtB->yrwcmd_i < -50.0F) {
4365 /* Transition: '<S323>:582' */
4366 _rtB->aplatL_a = 4;
4367 _rtB->psdcmd_g = 10.0F;
4368 _rtB->phiLcmd_l4 = 0.0F;
4369 } else {
4370 /* Transition: '<S323>:580' */
4371 _rtB->aplatL_a = 5;
4372 _rtB->yrwcmd_i = 0.0F;
4373 _rtB->psdcmd_g = 0.0F;
4374 _rtB->phiLcmd_l4 = 0.0F;
4375 }
4376 } else {
4377 /* During 'phiLcontrol2': '<S323>:571' */
4378 if ((((int32_T *)ssGetDWork(S, 315))[0] == 5) && (_rtB->SWAUTO == 1))
4379 {
4380 /* Transition: '<S323>:570' */
4381 _rtB->StartEmNav_o = 1;
4382 ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_YrwControl_o;
4383
4384 /* Entry 'YrwControl': '<S323>:577' */
4385 ((int32_T *)ssGetDWork(S, 316))[0] = 0;
4386 } else {
4387 ((int32_T *)ssGetDWork(S, 315))[0] = AFCS_MODEL1_addup_f((real32_T)
4388 fabs(AFCS_MODEL1_res180_l(_rtB->DetaPsi)) < 15.0F, 5, ((int32_T *)
4389 ssGetDWork(S, 315))[0]);
4390
4391 /* Transition: '<S323>:573' */
4392 if (AFCS_MODEL1_res180_l(_rtB->DetaPsi) > 15.0F) {
4393 /* Transition: '<S323>:574' */
4394 _rtB->aplatL_a = 2;
4395 _rtB->phiLcmd_l4 = 30.0F;
4396 } else {
4397 if (AFCS_MODEL1_res180_l(_rtB->DetaPsi) < -15.0F) {
4398 /* Transition: '<S323>:576' */
4399 _rtB->aplatL_a = 2;
4400 _rtB->phiLcmd_l4 = -30.0F;
4401 } else {
4402 /* Transition: '<S323>:575' */
4403 _rtB->aplatL_a = 2;
4404 _rtB->phiLcmd_l4 = 0.0F;
4405 }
4406 }
4407 }
4408 }
4409 break;
4410
4411 case AFCS_MODEL1_IN_PhiLControl3:
4412 /* During 'PhiLControl3': '<S323>:527' */
4413 if (((int32_T *)ssGetDWork(S, 318))[0] == 5) {
4414 /* Transition: '<S323>:536' */
4415 ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL1_IN_TranseNorm;
4416
4417 /* Entry 'TranseNorm': '<S323>:510' */
4418 _rtB->StartEmNav_o = 0;
4419 ((int8_T *)ssGetDWork(S, 513))[0] = 1;
4420 tmp = ((int32_T *)ssGetDWork(S, 317))[0] + 1LL;
4421 if (tmp > 2147483647LL) {
4422 tmp = 2147483647LL;
4423 } else {
4424 if (tmp < -2147483648LL) {
4425 tmp = -2147483648LL;
4426 }
4427 }
4428
4429 ((int32_T *)ssGetDWork(S, 317))[0] = (int32_T)tmp;
4430 } else {
4431 _rtB->phiLcmd_l4 = 30.0F * _rtB->DireFg;
4432 ((int32_T *)ssGetDWork(S, 318))[0] = AFCS_MODEL1_addup_f((real32_T)
4433 fabs(AFCS_MODEL1_res180_l(_rtB->DetaPsi)) < 15.0F, 5, ((int32_T *)
4434 ssGetDWork(S, 318))[0]);
4435 }
4436 break;
4437
4438 default:
4439 /* During 'TranseNorm': '<S323>:510' */
4440 break;
4441 }
4442 }
4443 } else {
4444 /* During 'NormYrwControl': '<S323>:454' */
4445 if ((_rtB->EmlandFg == 1) && (((int32_T *)ssGetDWork(S, 317))[0] == 0) &&
4446 (_rtB->GPSFail == 0)) {
4447 /* Transition: '<S323>:495' */
4448 /* Exit Internal 'NormYrwControl': '<S323>:454' */
4449 ((uint8_T *)ssGetDWork(S, 553))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
4450
4451 /* Exit Internal 'YrwControl1': '<S323>:550' */
4452 ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
4453 ((uint8_T *)ssGetDWork(S, 551))[0] = AFCS_MODEL1_IN_EmYrwControl;
4454
4455 /* Entry Internal 'EmYrwControl': '<S323>:493' */
4456 /* Transition: '<S323>:502' */
4457 ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL1_IN_LevelPosition;
4458
4459 /* Entry Internal 'LevelPosition': '<S323>:567' */
4460 /* Transition: '<S323>:568' */
4461 ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_phiLcontrol2;
4462
4463 /* Entry 'phiLcontrol2': '<S323>:571' */
4464 _rtB->aplatL_a = 2;
4465 _rtB->apyawL_fh = 1;
4466 ((int32_T *)ssGetDWork(S, 315))[0] = 0;
4467 ((int8_T *)ssGetDWork(S, 513))[0] = 0;
4468 } else if (((uint8_T *)ssGetDWork(S, 553))[0] ==
4469 AFCS_MODEL_IN_GPSFailPhiControl) {
4470 /* During 'GPSFailPhiControl': '<S323>:545' */
4471 if (_rtB->GPSFail == 0) {
4472 /* Transition: '<S323>:544' */
4473 ((uint8_T *)ssGetDWork(S, 553))[0] = AFCS_MODEL1_IN_YrwControl1;
4474
4475 /* Entry Internal 'YrwControl1': '<S323>:550' */
4476 /* Transition: '<S323>:551' */
4477 ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_phiLcontrol;
4478
4479 /* Entry 'phiLcontrol': '<S323>:554' */
4480 _rtB->aplatL_a = 5;
4481 _rtB->apyawL_fh = 1;
4482 _rtB->psdcmd_g = 0.0F;
4483 ((int32_T *)ssGetDWork(S, 314))[0] = 0;
4484 }
4485 } else {
4486 /* During 'YrwControl1': '<S323>:550' */
4487 if (_rtB->GPSFail == 1) {
4488 /* Transition: '<S323>:543' */
4489 /* Exit Internal 'YrwControl1': '<S323>:550' */
4490 ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
4491 ((uint8_T *)ssGetDWork(S, 553))[0] = AFCS_MODEL_IN_GPSFailPhiControl;
4492
4493 /* Entry 'GPSFailPhiControl': '<S323>:545' */
4494 _rtB->aplatL_a = 2;
4495 _rtB->apyawL_fh = 1;
4496 _rtB->phiLcmd_l4 = 30.0F;
4497 } else if (((uint8_T *)ssGetDWork(S, 554))[0] ==
4498 AFCS_MODEL1_IN_YrwControl_o) {
4499 /* During 'YrwControl': '<S323>:560' */
4500 /* Transition: '<S323>:562' */
4501 if (_rtB->yrw - _rtB->yrwcmd_i > 50.0F) {
4502 /* Transition: '<S323>:564' */
4503 _rtB->aplatL_a = 4;
4504 _rtB->psdcmd_g = -10.0F;
4505 _rtB->phiLcmd_l4 = 0.0F;
4506 } else if (_rtB->yrw - _rtB->yrwcmd_i < -50.0F) {
4507 /* Transition: '<S323>:565' */
4508 _rtB->aplatL_a = 4;
4509 _rtB->psdcmd_g = 10.0F;
4510 _rtB->phiLcmd_l4 = 0.0F;
4511 } else {
4512 /* Transition: '<S323>:563' */
4513 _rtB->aplatL_a = 5;
4514 _rtB->yrwcmd_i = 0.0F;
4515 _rtB->psdcmd_g = 0.0F;
4516 _rtB->phiLcmd_l4 = 0.0F;
4517 }
4518 } else {
4519 /* During 'phiLcontrol': '<S323>:554' */
4520 if ((((int32_T *)ssGetDWork(S, 314))[0] == 5) && (_rtB->SWAUTO == 1)) {
4521 /* Transition: '<S323>:552' */
4522 ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_YrwControl_o;
4523 } else {
4524 ((int32_T *)ssGetDWork(S, 314))[0] = AFCS_MODEL1_addup_f((real32_T)
4525 fabs(AFCS_MODEL1_res180_l(_rtB->psdcmd_g - _rtB->psd)) < 15.0F, 5,
4526 ((int32_T *)ssGetDWork(S, 314))[0]);
4527
4528 /* Transition: '<S323>:556' */
4529 if (AFCS_MODEL1_res180_l(_rtB->psdcmd_g - _rtB->psd) > 20.0F) {
4530 /* Transition: '<S323>:557' */
4531 _rtB->aplatL_a = 2;
4532 _rtB->phiLcmd_l4 = 30.0F;
4533 } else {
4534 if (AFCS_MODEL1_res180_l(_rtB->psdcmd_g - _rtB->psd) < -20.0F) {
4535 /* Transition: '<S323>:559' */
4536 _rtB->aplatL_a = 2;
4537 _rtB->phiLcmd_l4 = -30.0F;
4538 } else {
4539 /* Transition: '<S323>:558' */
4540 _rtB->aplatL_a = 4;
4541 _rtB->psdcmd_g = 0.0F;
4542 _rtB->phiLcmd_l4 = 0.0F;
4543 }
4544 }
4545 }
4546 }
4547 }
4548 }
4549}
4550
4551/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */
4552static void AFCS_MODEL1_tand(real_T *x)
4553{
4554 int8_T n;
4555 static real_T b_x;
4556 static real_T absx;
4557 if (!!rtIsInf(*x)) {
4558 *x = (rtNaN);
4559 } else {
4560 b_x = AFCS_MODEL1_rt_remd_snf(*x, 360.0);
4561 absx = fabs(b_x);
4562 if (absx > 180.0) {
4563 if (b_x > 0.0) {
4564 b_x -= 360.0;
4565 } else {
4566 b_x += 360.0;
4567 }
4568
4569 absx = fabs(b_x);
4570 }
4571
4572 if (absx <= 45.0) {
4573 b_x *= 0.017453292519943295;
4574 n = 0;
4575 } else if (absx <= 135.0) {
4576 if (b_x > 0.0) {
4577 b_x = (b_x - 90.0) * 0.017453292519943295;
4578 n = 1;
4579 } else {
4580 b_x = (b_x + 90.0) * 0.017453292519943295;
4581 n = -1;
4582 }
4583 } else if (b_x > 0.0) {
4584 b_x = (b_x - 180.0) * 0.017453292519943295;
4585 n = 2;
4586 } else {
4587 b_x = (b_x + 180.0) * 0.017453292519943295;
4588 n = -2;
4589 }
4590
4591 *x = tan(b_x);
4592 if ((n == 1) || (n == -1)) {
4593 b_x = 1.0 / *x;
4594 *x = -(1.0 / *x);
4595 if (rtIsInf(*x) && (n == 1)) {
4596 *x = -(-b_x);
4597 }
4598 }
4599 }
4600}
4601
4602/* Function for Chart: '<S314>/Climb2' */
4603static real32_T AFCS_MODEL1_res180(real32_T angle)
4604{
4605 real32_T outangle;
4606
4607 /* MATLAB Function 'res180': '<S324>:229' */
4608 /* Graphical Function 'res180': '<S324>:229' */
4609 /* '<S324>:235:1' */
4610 if (angle > 180.0F) {
4611 /* '<S324>:235:1' */
4612 outangle = angle - 360.0F;
4613 } else {
4614 /* '<S324>:237:1' */
4615 if (angle < -180.0F) {
4616 /* '<S324>:237:1' */
4617 outangle = angle + 360.0F;
4618 } else {
4619 /* '<S324>:236:1' */
4620 outangle = angle;
4621 }
4622 }
4623
4624 return outangle;
4625}
4626
4627/* Function for Chart: '<S315>/Cruise' */
4628static int32_T AFCS_MODEL1_addup_o(boolean_T bA, int32_T iB, int32_T iC)
4629{
4630 int32_T tC;
4631 int64_T tmp;
4632
4633 /* MATLAB Function 'addup': '<S325>:711' */
4634 /* Graphical Function 'addup': '<S325>:711' */
4635 /* '<S325>:719:1' */
4636 tC = iC;
4637
4638 /* '<S325>:720:1' */
4639 if (!bA) {
4640 /* '<S325>:723:1' */
4641 if (iC > 0) {
4642 /* '<S325>:723:1' */
4643 tC = iC - 1;
4644 }
4645 } else {
4646 /* '<S325>:725:1' */
4647 if (iC > iB) {
4648 /* '<S325>:725:1' */
4649 tC = iB;
4650 } else {
4651 /* '<S325>:724:1' */
4652 tmp = iC + 1LL;
4653 if (tmp > 2147483647LL) {
4654 tmp = 2147483647LL;
4655 } else {
4656 if (tmp < -2147483648LL) {
4657 tmp = -2147483648LL;
4658 }
4659 }
4660
4661 tC = (int32_T)tmp;
4662 }
4663 }
4664
4665 return tC;
4666}
4667
4668/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */
4669static void AFCS_MODEL1_PGT_CalculateRmin(const real_T xValues[32], const real_T
4670 yValues[32], int8_T IndataLength, real_T Rmin[32], real_T b_RminNew[255])
4671{
4672 static real_T dcwp[255];
4673 static real_T dwp;
4674 static real_T ax;
4675 static real_T ay;
4676 static real_T bx;
4677 static real_T by;
4678 static real_T Cthet;
4679 int32_T count;
4680 int8_T i;
4681 int32_T tmp;
4682
4683 /* UNTITLED3 此处显示有关此函数的摘要 */
4684 /* 此处显示详细说明 */
4685 /* 对转弯半径进行微调,避免转弯弧线有重叠 */
4686 memset(&dcwp[0], 0, 255U * sizeof(real_T));
4687 tmp = IndataLength - 2;
4688 if (tmp < -128) {
4689 tmp = -128;
4690 }
4691
4692 for (i = 1; i <= (int8_T)tmp; i++) {
4693 dwp = sqrt((xValues[i] - xValues[i - 1]) * (xValues[i] - xValues[i - 1]) +
4694 (yValues[i] - yValues[i - 1]) * (yValues[i] - yValues[i - 1]));
4695
4696 /* (ax,ay)为中间航点指向前一航点的向量 */
4697 ax = xValues[i - 1] - xValues[i];
4698 ay = yValues[i - 1] - yValues[i];
4699
4700 /* (bx,by)为中间航点指向后一航点的向量 */
4701 bx = xValues[i + 1] - xValues[i];
4702 by = yValues[i + 1] - yValues[i];
4703 if (sqrt(ax * ax + ay * ay) * sqrt(bx * bx + by * by) < 1.0E-7) {
4704 /* 若有两点重合 */
4705 Cthet = 1.0E+7;
4706 } else {
4707 Cthet = (ax * bx + ay * by) / (sqrt(ax * ax + ay * ay) * sqrt(bx * bx + by
4708 * by));
4709
4710 /* //两向量夹角的余弦值 */
4711 if (1.0 + Cthet < 1.0E-7) {
4712 Cthet = 1.0E+7;
4713 } else {
4714 Cthet = sqrt((1.0 - Cthet) / (1.0 + Cthet));
4715
4716 /* //两向量夹角一半的正切值 */
4717 }
4718 }
4719
4720 /* //当三个航点有两个重合,或者两向量夹角为pi,或者两向量夹角为零 */
4721 if ((sqrt(ax * ax + ay * ay) * sqrt(bx * bx + by * by) < 1.0E-7) || (fabs
4722 (atan(Cthet) - 1.5707963267948966) < 1.0E-7) || (fabs(Cthet) < 0.1)) {
4723 /* 2018/10/22修改 */
4724 dcwp[i] = 0.0;
4725
4726 /* //在这三个航点处转弯所需的路径长度为零 */
4727 } else {
4728 dcwp[i] = Rmin[i] / Cthet;
4729
4730 /* //在这三个航点处转弯所需的路径长度为最小转弯半径除以夹角一半的正切值 */
4731 }
4732
4733 /* 判断当前转弯半径是否满足条件,若不满足,则减小转弯半径 */
4734 count = 0;
4735 while ((dwp < dcwp[i - 1] + dcwp[i]) && (count < 100)) {
4736 /* 2018/10/09修改 */
4737 Rmin[i] *= 0.95;
4738 Rmin[i - 1] *= 0.95;
4739 dcwp[i - 1] *= 0.95;
4740 dcwp[i] *= 0.95;
4741 count++;
4742 }
4743 }
4744
4745 memset(&b_RminNew[0], 0, 255U * sizeof(real_T));
4746 for (i = 1; i <= IndataLength; i++) {
4747 b_RminNew[i - 1] = Rmin[i - 1];
4748 }
4749}
4750
4751static void rt_mrdivide_U1d1x3_U2d_9vOrDY_k(const real_T u0[3], const real_T u1
4752 [9], real_T y[3])
4753{
4754 static real_T A[9];
4755 int32_T r1;
4756 int32_T r2;
4757 int32_T r3;
4758 static real_T maxval;
4759 static real_T a21;
4760 int32_T rtemp;
4761 memcpy(&A[0], &u1[0], 9U * sizeof(real_T));
4762 r1 = 0;
4763 r2 = 1;
4764 r3 = 2;
4765 maxval = fabs(u1[0]);
4766 a21 = fabs(u1[1]);
4767 if (a21 > maxval) {
4768 maxval = a21;
4769 r1 = 1;
4770 r2 = 0;
4771 }
4772
4773 if (fabs(u1[2]) > maxval) {
4774 r1 = 2;
4775 r2 = 1;
4776 r3 = 0;
4777 }
4778
4779 A[r2] = u1[r2] / u1[r1];
4780 A[r3] /= A[r1];
4781 A[3 + r2] -= A[3 + r1] * A[r2];
4782 A[3 + r3] -= A[3 + r1] * A[r3];
4783 A[6 + r2] -= A[6 + r1] * A[r2];
4784 A[6 + r3] -= A[6 + r1] * A[r3];
4785 if (fabs(A[3 + r3]) > fabs(A[3 + r2])) {
4786 rtemp = r2 + 1;
4787 r2 = r3;
4788 r3 = rtemp - 1;
4789 }
4790
4791 A[3 + r3] /= A[3 + r2];
4792 A[6 + r3] -= A[3 + r3] * A[6 + r2];
4793 y[r1] = u0[0] / A[r1];
4794 y[r2] = u0[1] - A[3 + r1] * y[r1];
4795 y[r3] = u0[2] - A[6 + r1] * y[r1];
4796 y[r2] /= A[3 + r2];
4797 y[r3] -= A[6 + r2] * y[r2];
4798 y[r3] /= A[6 + r3];
4799 y[r2] -= A[3 + r3] * y[r3];
4800 y[r1] -= y[r3] * A[r3];
4801 y[r1] -= y[r2] * A[r2];
4802}
4803
4804/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */
4805static void AFCS_MODEL1_PGT_changewaypoints(const real_T xValues[32], const
4806 real_T yValues[32], const real_T hValues[32], int8_T IndataLength, const
4807 real_T Rmin[255], const real_T vValues[32], real_T vx_qiedian_out[255], real_T
4808 vy_qiedian_out[255], real_T xn_qiedian_out[255], real_T ye_qiedian_out[255],
4809 real_T zh_qiedian_out[255])
4810{
4811 static real_T K;
4812 static real_T d_to_front;
4813 static real_T q1x;
4814 static real_T q1y;
4815 static real_T d_to_next;
4816 static real_T q2x;
4817 static real_T q2y;
4818 static real_T Anglerou;
4819 static real_T Angleroucos;
4820 int8_T i;
4821 static real_T a;
4822 int32_T tmp;
4823 int32_T tmp_0;
4824
4825 /* 输入: */
4826 /* 1.xValues:航点的x值 */
4827 /* 2.yValues:航点的y值 */
4828 /* 3.zValues:航点的z值 */
4829 /* 4.IndataLength:航点的个数 */
4830 /* 5.Rmin:各个航点的最小转弯半径 */
4831 /* 6.vValues:每个航点的速度大小 */
4832 /* 输出: */
4833 /* 1.vx:切点的x方向速度值 */
4834 /* 2.vy:切点的y方向速度值 */
4835 /* 3.xx:切点的x方向位置 */
4836 /* 4.yy:切点的y方向位置 */
4837 /* 5.zz:切点的z方向位置 */
4838 /* 备注:由三个航点(i-1,i,i+1)可以确定i航点的切点,这里的切点指i与i+1航点之间的切点 */
4839 vx_qiedian_out[254] = 0.0;
4840 vx_qiedian_out[0] = 0.0;
4841 vy_qiedian_out[254] = 0.0;
4842 vy_qiedian_out[0] = 0.0;
4843 xn_qiedian_out[254] = 0.0;
4844 xn_qiedian_out[0] = 0.0;
4845 ye_qiedian_out[254] = 0.0;
4846 ye_qiedian_out[0] = 0.0;
4847 memset(&zh_qiedian_out[0], 0, 255U * sizeof(real_T));
4848 memset(&vx_qiedian_out[1], 0, 253U * sizeof(real_T));
4849 memset(&vy_qiedian_out[1], 0, 253U * sizeof(real_T));
4850 memset(&xn_qiedian_out[1], 0, 253U * sizeof(real_T));
4851 memset(&ye_qiedian_out[1], 0, 253U * sizeof(real_T));
4852
4853 /* % */
4854 for (i = 1; i <= IndataLength; i++) {
4855 /* velocity */
4856 /* 还是对于垂直于x轴的情况要单独处理 */
4857 tmp_0 = i + 1;
4858 if (tmp_0 > 127) {
4859 tmp_0 = 127;
4860 }
4861
4862 if (fabs(xValues[tmp_0 - 1] - xValues[i - 1]) < 1.0E-7) {
4863 vx_qiedian_out[i - 1] = 0.0;
4864 tmp_0 = i + 1;
4865 if (tmp_0 > 127) {
4866 tmp_0 = 127;
4867 }
4868
4869 if (fabs(yValues[tmp_0 - 1] - yValues[i - 1]) < 1.0E-7) {
4870 vy_qiedian_out[i - 1] = 0.0;
4871 } else {
4872 tmp_0 = i + 1;
4873 if (tmp_0 > 127) {
4874 tmp_0 = 127;
4875 }
4876
4877 if (yValues[tmp_0 - 1] - yValues[i - 1] > 0.0) {
4878 vy_qiedian_out[i - 1] = vValues[i - 1];
4879 } else {
4880 vy_qiedian_out[i - 1] = -vValues[i - 1];
4881 }
4882 }
4883 } else {
4884 tmp_0 = i + 1;
4885 if (tmp_0 > 127) {
4886 tmp_0 = 127;
4887 }
4888
4889 tmp = i + 1;
4890 if (tmp > 127) {
4891 tmp = 127;
4892 }
4893
4894 K = (yValues[tmp_0 - 1] - yValues[i - 1]) / (xValues[tmp - 1] - xValues[i
4895 - 1]);
4896 tmp_0 = i + 1;
4897 if (tmp_0 > 127) {
4898 tmp_0 = 127;
4899 }
4900
4901 if (xValues[tmp_0 - 1] - xValues[i - 1] >= 0.0) {
4902 vx_qiedian_out[i - 1] = vValues[i - 1] / sqrt(K * K + 1.0);
4903 } else {
4904 vx_qiedian_out[i - 1] = -vValues[i - 1] / sqrt(K * K + 1.0);
4905 }
4906
4907 tmp_0 = i + 1;
4908 if (tmp_0 > 127) {
4909 tmp_0 = 127;
4910 }
4911
4912 if (yValues[tmp_0 - 1] - yValues[i - 1] >= 0.0) {
4913 vy_qiedian_out[i - 1] = vValues[i - 1] / sqrt(K * K + 1.0) * fabs(K);
4914 } else {
4915 vy_qiedian_out[i - 1] = -vValues[i - 1] / sqrt(K * K + 1.0) * fabs(K);
4916 }
4917 }
4918
4919 /* ===================================================== */
4920 /* a*b x1*x2+y1*y2 */
4921 /* COS(theta) =----------------- = --------------- */
4922 /* |a|*|b| |a|*|b| */
4923 /* ===================================================== */
4924 /* //waypoints// */
4925 if ((i == 1) || (i == IndataLength)) {
4926 xn_qiedian_out[i - 1] = xValues[i - 1];
4927
4928 /* yht:第一个航点和最后一个航点的切点位置就是航点本身 */
4929 ye_qiedian_out[i - 1] = yValues[i - 1];
4930 zh_qiedian_out[i - 1] = hValues[i - 1];
4931 } else {
4932 tmp_0 = i - 1;
4933 if (tmp_0 < -128) {
4934 tmp_0 = -128;
4935 }
4936
4937 a = xValues[i - 1] - xValues[tmp_0 - 1];
4938 tmp_0 = i - 1;
4939 if (tmp_0 < -128) {
4940 tmp_0 = -128;
4941 }
4942
4943 d_to_front = yValues[i - 1] - yValues[tmp_0 - 1];
4944 d_to_front = sqrt(a * a + d_to_front * d_to_front);
4945 if (d_to_front < 1.0E-7) {
4946 /* 当前航点与上一航点重合 */
4947 q1x = 0.0;
4948 q1y = 0.0;
4949 } else {
4950 tmp_0 = i - 1;
4951 if (tmp_0 < -128) {
4952 tmp_0 = -128;
4953 }
4954
4955 q1x = (xValues[tmp_0 - 1] - xValues[i - 1]) / d_to_front;
4956
4957 /* COS倾斜角(连线与x轴的夹角),归一化的相对坐标 */
4958 tmp_0 = i - 1;
4959 if (tmp_0 < -128) {
4960 tmp_0 = -128;
4961 }
4962
4963 q1y = (yValues[tmp_0 - 1] - yValues[i - 1]) / d_to_front;
4964
4965 /* SIN倾斜角 */
4966 }
4967
4968 tmp_0 = i + 1;
4969 if (tmp_0 > 127) {
4970 tmp_0 = 127;
4971 }
4972
4973 d_to_next = xValues[tmp_0 - 1] - xValues[i - 1];
4974 tmp_0 = i + 1;
4975 if (tmp_0 > 127) {
4976 tmp_0 = 127;
4977 }
4978
4979 q2x = yValues[tmp_0 - 1] - yValues[i - 1];
4980 d_to_next = sqrt(d_to_next * d_to_next + q2x * q2x);
4981 if (d_to_next < 1.0E-7) {
4982 q2x = 0.0;
4983 q2y = 0.0;
4984 } else {
4985 tmp_0 = i + 1;
4986 if (tmp_0 > 127) {
4987 tmp_0 = 127;
4988 }
4989
4990 q2x = (xValues[tmp_0 - 1] - xValues[i - 1]) / d_to_next;
4991 tmp_0 = i + 1;
4992 if (tmp_0 > 127) {
4993 tmp_0 = 127;
4994 }
4995
4996 q2y = (yValues[tmp_0 - 1] - yValues[i - 1]) / d_to_next;
4997 }
4998
4999 if ((fabs(q1x) + fabs(q1y) < 1.0E-7) || (fabs(q2x) + fabs(q2y) < 1.0E-7))
5000 {
5001 /* 如果有航点是重合的,则夹角是0 */
5002 Anglerou = 0.0;
5003 } else {
5004 Angleroucos = q1x * q2x + q1y * q2y;
5005 if (Angleroucos > 1.0) {
5006 Angleroucos = 1.0;
5007 } else {
5008 if (Angleroucos < -1.0) {
5009 Angleroucos = -1.0;
5010 }
5011 }
5012
5013 Anglerou = acos(Angleroucos);
5014 }
5015
5016 /* %% */
5017 /* 2018/10/09修改 */
5018 if ((fabs(Anglerou) < 0.087266462599716474) || (3.1415926535897931 -
5019 Anglerou < 0.087266462599716474)) {
5020 /* 两直线夹角小于5度时切点为原始航点 */
5021 xn_qiedian_out[i - 1] = xValues[i - 1];
5022 ye_qiedian_out[i - 1] = yValues[i - 1];
5023 } else {
5024 xn_qiedian_out[i - 1] = Rmin[i - 1] / tan(Anglerou / 2.0) * q2x +
5025 xValues[i - 1];
5026 ye_qiedian_out[i - 1] = Rmin[i - 1] / tan(Anglerou / 2.0) * q2y +
5027 yValues[i - 1];
5028 }
5029
5030 /* dd = sqrt((xx(i) - xValues(i))*(xx(i) - xValues(i)) + (yy(i) - yValues(i))*(yy(i) - yValues(i))); %切点到航点的距离 */
5031 /* L = sqrt((xValues(i + 1) - xValues(i))*(xValues(i + 1) - xValues(i)) + (yValues(i + 1) - yValues(i))*(yValues(i + 1) - yValues(i))); %两个航点的距离 */
5032 zh_qiedian_out[i - 1] = hValues[i - 1];
5033
5034 /* + dd / L*(zValues(i + 1) - zValues(i)); %切点的高度,yanhongtao,20210220 */
5035 }
5036 }
5037}
5038
5039/* Function for Chart: '<S21>/delayblock1' */
5040static int32_T AFCS_MODEL1_addup(int32_T bA, int32_T iB, int32_T iC)
5041{
5042 int32_T tC;
5043 int64_T tmp;
5044
5045 /* MATLAB Function 'addup': '<S48>:114' */
5046 /* Graphical Function 'addup': '<S48>:114' */
5047 /* '<S48>:122:1' */
5048 tC = iC;
5049
5050 /* '<S48>:123:1' */
5051 if (bA == 0) {
5052 /* '<S48>:126:1' */
5053 if (iC > 0) {
5054 /* '<S48>:126:1' */
5055 tC = iC - 1;
5056 }
5057 } else {
5058 /* '<S48>:128:1' */
5059 if (iC > iB) {
5060 /* '<S48>:128:1' */
5061 tC = iB;
5062 } else {
5063 /* '<S48>:127:1' */
5064 tmp = iC + 1LL;
5065 if (tmp > 2147483647LL) {
5066 tmp = 2147483647LL;
5067 } else {
5068 if (tmp < -2147483648LL) {
5069 tmp = -2147483648LL;
5070 }
5071 }
5072
5073 tC = (int32_T)tmp;
5074 }
5075 }
5076
5077 return tC;
5078}
5079
5080/* Function for MATLAB Function: '<S293>/CompTP' */
5081static void AFCS_MODEL1_CompDistance(real32_T X1, real32_T Y1, real32_T X2,
5082 real32_T Y2, real32_T Xac, real32_T Yac, real32_T *DD, real32_T *A, real32_T
5083 *Xrw, real32_T *Yrw)
5084{
5085 real32_T DX;
5086 real32_T DY;
5087 real32_T DXp;
5088 real32_T DYp;
5089
5090 /* '<S295>:1:138' */
5091 DX = X2 - X1;
5092
5093 /* '<S295>:1:139' */
5094 DY = Y2 - Y1;
5095
5096 /* '<S295>:1:140' */
5097 DXp = Xac - X1;
5098
5099 /* '<S295>:1:141' */
5100 DYp = Yac - Y1;
5101
5102 /* '<S295>:1:142' */
5103 *DD = (real32_T)sqrt(DX * DX + DY * DY);
5104
5105 /* '<S295>:1:143' */
5106 *A = AFCS_MODEL1_rt_atan2f_snf(DY, DX) * 180.0F / 3.14159274F;
5107
5108 /* '<S295>:1:144' */
5109 DX /= *DD;
5110
5111 /* '<S295>:1:145' */
5112 DY /= *DD;
5113
5114 /* '<S295>:1:146' */
5115 *Yrw = DYp * DX - DXp * DY;
5116
5117 /* '<S295>:1:147' */
5118 *Xrw = *DD - (DXp * DX + DYp * DY);
5119}
5120
5121/* Function for Chart: '<S21>/delayblock6' */
5122static int32_T AFCS_MODEL1_addup_m(int32_T bA, int32_T iB, int32_T iC)
5123{
5124 int32_T tC;
5125 int64_T tmp;
5126
5127 /* MATLAB Function 'addup': '<S53>:114' */
5128 /* Graphical Function 'addup': '<S53>:114' */
5129 /* '<S53>:122:1' */
5130 tC = iC;
5131
5132 /* '<S53>:123:1' */
5133 if (bA == 0) {
5134 /* '<S53>:126:1' */
5135 if (iC > 0) {
5136 /* '<S53>:126:1' */
5137 tC = iC - 1;
5138 }
5139 } else {
5140 /* '<S53>:128:1' */
5141 if (iC > iB) {
5142 /* '<S53>:128:1' */
5143 tC = iB;
5144 } else {
5145 /* '<S53>:127:1' */
5146 tmp = iC + 1LL;
5147 if (tmp > 2147483647LL) {
5148 tmp = 2147483647LL;
5149 } else {
5150 if (tmp < -2147483648LL) {
5151 tmp = -2147483648LL;
5152 }
5153 }
5154
5155 tC = (int32_T)tmp;
5156 }
5157 }
5158
5159 return tC;
5160}
5161
5162/* Function for MATLAB Function: '<S293>/CompTP' */
5163static void AFCS_MODEL1_cosd(real32_T *x)
5164{
5165 int8_T n;
5166 real32_T b_x;
5167 real32_T absx;
5168 if (!((!rtIsInfF(*x)) && (!rtIsNaNF(*x)))) {
5169 *x = (rtNaNF);
5170 } else {
5171 b_x = AFCS_MODEL1_rt_remf_snf(*x, 360.0F);
5172 absx = (real32_T)fabs(b_x);
5173 if (absx > 180.0F) {
5174 if (b_x > 0.0F) {
5175 b_x -= 360.0F;
5176 } else {
5177 b_x += 360.0F;
5178 }
5179
5180 absx = (real32_T)fabs(b_x);
5181 }
5182
5183 if (absx <= 45.0F) {
5184 b_x *= 0.0174532924F;
5185 n = 0;
5186 } else if (absx <= 135.0F) {
5187 if (b_x > 0.0F) {
5188 b_x = (b_x - 90.0F) * 0.0174532924F;
5189 n = 1;
5190 } else {
5191 b_x = (b_x + 90.0F) * 0.0174532924F;
5192 n = -1;
5193 }
5194 } else if (b_x > 0.0F) {
5195 b_x = (b_x - 180.0F) * 0.0174532924F;
5196 n = 2;
5197 } else {
5198 b_x = (b_x + 180.0F) * 0.0174532924F;
5199 n = -2;
5200 }
5201
5202 if (n == 0) {
5203 *x = (real32_T)cos(b_x);
5204 } else if (n == 1) {
5205 *x = -(real32_T)sin(b_x);
5206 } else if (n == -1) {
5207 *x = (real32_T)sin(b_x);
5208 } else {
5209 *x = -(real32_T)cos(b_x);
5210 }
5211 }
5212}
5213
5214/* Function for MATLAB Function: '<S335>/UAV Trajectory Tracking' */
5215static void PGT_followOrbit_nonlinear_guida(real_T cn, real_T ce, real_T roup,
5216 real_T lamda, real_T pn, real_T pe, real_T vn, real_T ve, real_T psi, real_T
5217 zz, real_T nav_param, real_T *hc, real_T *psic, real_T *acmd)
5218{
5219 static real_T Vg;
5220 static real_T L1;
5221 static real_T d;
5222 static real_T dthet;
5223 real_T fi;
5224
5225 /* UNTITLED3 此处显示有关此函数的摘要 */
5226 /* Input: Orbit center c = (cn, ce, cd)T , radius roup, and direction lamda, MAV position p = (pn, pe, pd)T , */
5227 /* course psi, gains korbit. */
5228 /* 输入: */
5229 /* cn,ce:圆的圆心坐标x,y */
5230 /* roup:圆的半径 */
5231 /* lamda:圆的转动方向(顺时针还是逆时针) */
5232 /* pn,pe,vn,ve:飞机的当前位置和速度 */
5233 /* psi:飞机的速度偏角 */
5234 /* zz:飞机当前转弯点的z */
5235 /* nav_param:导航参数 */
5236 /* 此处显示详细说明 */
5237 Vg = sqrt(vn * vn + ve * ve);
5238
5239 /* 飞机的速度 */
5240 L1 = nav_param * Vg;
5241
5242 /* 飞机到预瞄点的距离 */
5243 d = pn - cn;
5244 dthet = pe - ce;
5245 d = sqrt(d * d + dthet * dthet);
5246
5247 /* 飞机到圆心的距离 */
5248 if (fabs(((d * d + roup * roup) - L1 * L1) / (2.0 * d * roup)) < 1.0) {
5249 dthet = acos(((d * d + roup * roup) - L1 * L1) / (2.0 * d * roup));
5250 } else {
5251 dthet = 0.005;
5252 }
5253
5254 *hc = -zz;
5255
5256 /* 指令高度为当前转弯点的高度 */
5257 fi = AFCS_MODEL1_rt_atan2d_snf(pe - ce, pn - cn);
5258
5259 /* 飞机与圆心的连线与北向的夹角 */
5260 if (fi < 0.0) {
5261 fi += 6.2831853071795862;
5262 }
5263
5264 d = cos(lamda * dthet + fi) * roup + cn;
5265
5266 /* 预瞄点的坐标x */
5267 dthet = sin(lamda * dthet + fi) * roup + ce;
5268
5269 /* 预瞄点的坐标y */
5270 /* 飞机实际到预瞄点的距离 */
5271 *psic = AFCS_MODEL1_rt_atan2d_snf(dthet - pe, d - pn);
5272
5273 /* atan2的范围是-pi~pi,飞机到预瞄点连线与北向的夹角 */
5274 if (*psic < 0.0) {
5275 *psic += 6.2831853071795862;
5276 }
5277
5278 /* 速度与飞机预瞄点连线的夹角 */
5279 *acmd = 2.0 * Vg * Vg / sqrt((pn - d) * (pn - d) + (pe - dthet) * (pe - dthet))
5280 * sin(*psic - psi);
5281
5282 /* 非线性制导 */
5283 if (*psic - psi < -3.1415926535897931) {
5284 *psic += 6.2831853071795862;
5285 }
5286
5287 if (*psic - psi > 3.1415926535897931) {
5288 *psic -= 6.2831853071795862;
5289 }
5290}
5291
5292/* Function for MATLAB Function: '<S293>/CompTP' */
5293static void AFCS_MODEL1_sind(real32_T *x)
5294{
5295 int8_T n;
5296 real32_T c_x;
5297 real32_T absx;
5298 if (!((!rtIsInfF(*x)) && (!rtIsNaNF(*x)))) {
5299 c_x = (rtNaNF);
5300 } else {
5301 c_x = AFCS_MODEL1_rt_remf_snf(*x, 360.0F);
5302 absx = (real32_T)fabs(c_x);
5303 if (absx > 180.0F) {
5304 if (c_x > 0.0F) {
5305 c_x -= 360.0F;
5306 } else {
5307 c_x += 360.0F;
5308 }
5309
5310 absx = (real32_T)fabs(c_x);
5311 }
5312
5313 if (absx <= 45.0F) {
5314 c_x *= 0.0174532924F;
5315 n = 0;
5316 } else if (absx <= 135.0F) {
5317 if (c_x > 0.0F) {
5318 c_x = (c_x - 90.0F) * 0.0174532924F;
5319 n = 1;
5320 } else {
5321 c_x = (c_x + 90.0F) * 0.0174532924F;
5322 n = -1;
5323 }
5324 } else if (c_x > 0.0F) {
5325 c_x = (c_x - 180.0F) * 0.0174532924F;
5326 n = 2;
5327 } else {
5328 c_x = (c_x + 180.0F) * 0.0174532924F;
5329 n = -2;
5330 }
5331
5332 if (n == 0) {
5333 c_x = (real32_T)sin(c_x);
5334 } else if (n == 1) {
5335 c_x = (real32_T)cos(c_x);
5336 } else if (n == -1) {
5337 c_x = -(real32_T)cos(c_x);
5338 } else {
5339 c_x = -(real32_T)sin(c_x);
5340 }
5341 }
5342
5343 *x = c_x;
5344}
5345
5346/* Function for MATLAB Function: '<S9>/MATLAB Function2' */
5347static void AFCS_MODEL1_cosd_i(real_T *x)
5348{
5349 int8_T n;
5350 static real_T b_x;
5351 static real_T absx;
5352 if (!((!rtIsInf(*x)) && (!rtIsNaN(*x)))) {
5353 *x = (rtNaN);
5354 } else {
5355 b_x = AFCS_MODEL1_rt_remd_snf(*x, 360.0);
5356 absx = fabs(b_x);
5357 if (absx > 180.0) {
5358 if (b_x > 0.0) {
5359 b_x -= 360.0;
5360 } else {
5361 b_x += 360.0;
5362 }
5363
5364 absx = fabs(b_x);
5365 }
5366
5367 if (absx <= 45.0) {
5368 b_x *= 0.017453292519943295;
5369 n = 0;
5370 } else if (absx <= 135.0) {
5371 if (b_x > 0.0) {
5372 b_x = (b_x - 90.0) * 0.017453292519943295;
5373 n = 1;
5374 } else {
5375 b_x = (b_x + 90.0) * 0.017453292519943295;
5376 n = -1;
5377 }
5378 } else if (b_x > 0.0) {
5379 b_x = (b_x - 180.0) * 0.017453292519943295;
5380 n = 2;
5381 } else {
5382 b_x = (b_x + 180.0) * 0.017453292519943295;
5383 n = -2;
5384 }
5385
5386 if (n == 0) {
5387 *x = cos(b_x);
5388 } else if (n == 1) {
5389 *x = -sin(b_x);
5390 } else if (n == -1) {
5391 *x = sin(b_x);
5392 } else {
5393 *x = -cos(b_x);
5394 }
5395 }
5396}
5397
5398/* Function for MATLAB Function: '<S9>/MATLAB Function2' */
5399static void AFCS_MODEL1_sind_k(real_T *x)
5400{
5401 int8_T n;
5402 static real_T c_x;
5403 static real_T absx;
5404 if (!((!rtIsInf(*x)) && (!rtIsNaN(*x)))) {
5405 c_x = (rtNaN);
5406 } else {
5407 c_x = AFCS_MODEL1_rt_remd_snf(*x, 360.0);
5408 absx = fabs(c_x);
5409 if (absx > 180.0) {
5410 if (c_x > 0.0) {
5411 c_x -= 360.0;
5412 } else {
5413 c_x += 360.0;
5414 }
5415
5416 absx = fabs(c_x);
5417 }
5418
5419 if (absx <= 45.0) {
5420 c_x *= 0.017453292519943295;
5421 n = 0;
5422 } else if (absx <= 135.0) {
5423 if (c_x > 0.0) {
5424 c_x = (c_x - 90.0) * 0.017453292519943295;
5425 n = 1;
5426 } else {
5427 c_x = (c_x + 90.0) * 0.017453292519943295;
5428 n = -1;
5429 }
5430 } else if (c_x > 0.0) {
5431 c_x = (c_x - 180.0) * 0.017453292519943295;
5432 n = 2;
5433 } else {
5434 c_x = (c_x + 180.0) * 0.017453292519943295;
5435 n = -2;
5436 }
5437
5438 if (n == 0) {
5439 c_x = sin(c_x);
5440 } else if (n == 1) {
5441 c_x = cos(c_x);
5442 } else if (n == -1) {
5443 c_x = -cos(c_x);
5444 } else {
5445 c_x = -sin(c_x);
5446 }
5447 }
5448
5449 *x = c_x;
5450}
5451
5452/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */
5453static void AFCS_MODEL1_PGT_circledot(real_T x, real_T y, real_T b_vx, real_T
5454 b_vy, real_T r, real_T *x_dot, real_T *y_dot, real_T *fi)
5455{
5456 real_T thet;
5457
5458 /* UNTITLED3 Summary of this function goes here */
5459 /* Detailed explanation goes here */
5460 /* YHT:求圆心坐标 */
5461 /* x,y,vx,vy:航点的位置坐标和速度 */
5462 /* r:圆的半径 */
5463 /* flag=1,表示顺时针,flag=0,表示逆时针 */
5464 if (b_vx >= 0.0) {
5465 if (fabs(b_vy) < 1.0E-7) {
5466 thet = 0.0;
5467 } else {
5468 thet = (atan(b_vy / b_vx) + 6.2831853071795862) / 6.2831853071795862;
5469 if (fabs(thet - AFCS_MODEL1_rt_roundd_snf(thet)) <= 2.2204460492503131E-16
5470 * fabs(thet)) {
5471 thet = 0.0;
5472 } else {
5473 thet = (thet - floor(thet)) * 6.2831853071795862;
5474 }
5475 }
5476 } else if (fabs(b_vy) < 1.0E-7) {
5477 thet = 3.1415926535897931;
5478 } else {
5479 thet = ((atan(b_vy / b_vx) + 3.1415926535897931) + 6.2831853071795862) /
5480 6.2831853071795862;
5481 if (fabs(thet - AFCS_MODEL1_rt_roundd_snf(thet)) <= 2.2204460492503131E-16 *
5482 fabs(thet)) {
5483 thet = 0.0;
5484 } else {
5485 thet = (thet - floor(thet)) * 6.2831853071795862;
5486 }
5487
5488 /* thet是切点处速度方向与北向的夹角,顺时针为正,0~360,yht */
5489 }
5490
5491 *x_dot = x - cos(thet + 1.5707963267948966) * r;
5492 *y_dot = y - sin(thet + 1.5707963267948966) * r;
5493 *fi = ((thet + 1.5707963267948966) + 6.2831853071795862) / 6.2831853071795862;
5494 if (fabs(*fi - AFCS_MODEL1_rt_roundd_snf(*fi)) <= 2.2204460492503131E-16 *
5495 fabs(*fi)) {
5496 *fi = 0.0;
5497 } else {
5498 *fi = (*fi - floor(*fi)) * 6.2831853071795862;
5499 }
5500}
5501
5502/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */
5503static void AFCS_MODEL1_PGT_Dubins_2D(real_T x1, real_T b_y1, real_T v1x, real_T
5504 v1y, real_T x2, real_T y2, real_T v2x, real_T v2y, real_T r_min, real_T *xq1,
5505 real_T *yq1, real_T *fis, real_T *fi1, real_T *xq2, real_T *yq2, real_T *fif,
5506 real_T *fi2, real_T *L, real_T *A, real_T *B, real_T *C, real_T *x0_dot,
5507 real_T *y0_dot, real_T *tx_dot, real_T *ty_dot)
5508{
5509 static real_T fi2_rw_in_sum;
5510 static real_T b_x0_dot;
5511 static real_T b_y0_dot;
5512 static real_T b_fis;
5513 static real_T b_tx_dot;
5514 static real_T b_ty_dot;
5515 static real_T b_fif;
5516 static real_T fi1_rw_out_sum;
5517 static real_T xq12_rw_out;
5518 static real_T yq12_rw_out;
5519 static real_T xq12_rw_in;
5520 static real_T yq12_rw_in;
5521 static real_T L12_rw_out;
5522 static real_T c;
5523 static real_T afa;
5524
5525 /* UNTITLED 此处显示有关此函数的摘要 */
5526 /* 此处显示详细说明 */
5527 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
5528 /* xq1,yq1为圆1上切点的位置 */
5529 /* fis,fi1为圆1上起点和切出点的位置 */
5530 /* Lamda1表示圆1的方向,Lamda1=-1表示顺时针,Lamda1=1表示逆时针 */
5531 /* xq2,yq2为圆2上切点的位置 */
5532 /* fif,fi2为圆2上切入点和终点的位置 */
5533 /* Lamda2表示圆2的方向,Lamda2=-1表示顺时针,Lamda2=1表示逆时针 */
5534 /* L切线段长度。 */
5535 /* A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */
5536 /* x0_dot,y0_dot,tx_dot,ty_dot为圆1和圆2的圆心坐标 */
5537 /* x1,y1,v1x,v1y,x2,y2,v2x,v2y,r_min分别对应起始点位置,速度,终止点位置,速度 */
5538 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
5539 /* %求圆心坐标 */
5540 if (fabs(v1x) + fabs(v1y) < 1.0E-7) {
5541 *x0_dot = x1;
5542 *y0_dot = b_y1;
5543 *fis = 0.0;
5544 } else {
5545 AFCS_MODEL1_PGT_circledot(x1, b_y1, v1x, v1y, r_min, &b_x0_dot, &b_y0_dot,
5546 &b_fis);
5547 *x0_dot = b_x0_dot;
5548 *y0_dot = b_y0_dot;
5549 *fis = b_fis;
5550
5551 /* 起始圆圆心坐标,fis是圆1上的起点角度 */
5552 }
5553
5554 if (fabs(v2x) + fabs(v2y) < 1.0E-7) {
5555 *tx_dot = x2;
5556 *ty_dot = y2;
5557 *fif = 0.0;
5558 } else {
5559 AFCS_MODEL1_PGT_circledot(x2, y2, v2x, v2y, r_min, &b_tx_dot, &b_ty_dot,
5560 &b_fif);
5561 *tx_dot = b_tx_dot;
5562 *ty_dot = b_ty_dot;
5563 *fif = b_fif;
5564
5565 /* 目标圆圆心坐标 */
5566 }
5567
5568 /* %求路径长度及相应坐标 */
5569 /* UNTITLED2 Summary of this function goes here */
5570 /* 计算两圆之间的dubins路径长度,返回长度值和圆2的切入点位置 */
5571 /* 输入: */
5572 /* fis1: */
5573 /* x1,y1,r1:圆1的圆心坐标及半径 */
5574 /* flag1:圆1的转动方向标志 */
5575 /* x2,y2,r2:圆2的圆心坐标及半径 */
5576 /* flag2:圆2的转动方向标志 */
5577 /* 输出: */
5578 /* */
5579 /* Detailed explanation goes here */
5580 /* UNTITLED2 Summary of this function goes here */
5581 /* Detailed explanation goes here */
5582 /* flag1=0,表示圆1在切线右边,flag1=1,表示圆1在切线左边; */
5583 /* flag2=0,表示外切线,flag2=1,表示内切线。 */
5584 /* A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */
5585 /* L切线段长度。 */
5586 /* fi1表示圆1上切点的位置,fi2表示圆2上切点的位置。 */
5587 /* 输入: */
5588 /* x1,y1,r1:圆1的圆心坐标及半径 */
5589 /* flag1:圆1的转动方向标志 */
5590 /* x2,y2,r2:圆2的圆心坐标及半径 */
5591 /* flag2:圆2的转动方向标志 */
5592 if ((*x0_dot == *tx_dot) && (*y0_dot == *ty_dot)) {
5593 /* 如果圆1和圆2重合 */
5594 xq12_rw_out = 0.0;
5595 yq12_rw_out = 0.0;
5596 *fi1 = 0.0;
5597 xq12_rw_in = 0.0;
5598 yq12_rw_in = 0.0;
5599 *fi2 = 0.0;
5600 L12_rw_out = 1.0E+7;
5601 *A = 1.0;
5602 *B = 1.0;
5603 *C = 1.0;
5604 } else {
5605 c = *x0_dot - *tx_dot;
5606 *fi1 = *y0_dot - *ty_dot;
5607 c = sqrt(c * c + *fi1 * *fi1);
5608
5609 /* 两圆的距离 */
5610 /* 外切线存在条件d_w<0; */
5611 /* 内切线存在条件d_n<0; */
5612 /* 圆在切线的右侧 */
5613 if (fabs(r_min - r_min) - c < 0.0) {
5614 /* 外切 */
5615 afa = asin((r_min - r_min) / c);
5616 *fi2 = AFCS_MODEL1_rt_atan2d_snf(*ty_dot - *y0_dot, *tx_dot - *x0_dot);
5617 *fi1 = (((afa + *fi2) + 1.5707963267948966) + 6.2831853071795862) /
5618 6.2831853071795862;
5619 if (fabs(*fi1 - AFCS_MODEL1_rt_roundd_snf(*fi1)) <= 2.2204460492503131E-16
5620 * fabs(*fi1)) {
5621 *fi1 = 0.0;
5622 } else {
5623 *fi1 = (*fi1 - floor(*fi1)) * 6.2831853071795862;
5624 }
5625
5626 *fi2 = (((afa + *fi2) + 1.5707963267948966) + 6.2831853071795862) /
5627 6.2831853071795862;
5628 if (fabs(*fi2 - AFCS_MODEL1_rt_roundd_snf(*fi2)) <= 2.2204460492503131E-16
5629 * fabs(*fi2)) {
5630 *fi2 = 0.0;
5631 } else {
5632 *fi2 = (*fi2 - floor(*fi2)) * 6.2831853071795862;
5633 }
5634
5635 xq12_rw_out = r_min * cos(*fi1) + *x0_dot;
5636 yq12_rw_out = r_min * sin(*fi1) + *y0_dot;
5637 xq12_rw_in = r_min * cos(*fi2) + *tx_dot;
5638 yq12_rw_in = r_min * sin(*fi2) + *ty_dot;
5639 L12_rw_out = xq12_rw_out - xq12_rw_in;
5640 fi1_rw_out_sum = yq12_rw_out - yq12_rw_in;
5641 L12_rw_out = sqrt(L12_rw_out * L12_rw_out + fi1_rw_out_sum *
5642 fi1_rw_out_sum);
5643 } else {
5644 *fi1 = 0.0;
5645 *fi2 = 0.0;
5646 xq12_rw_out = 0.0;
5647 yq12_rw_out = 0.0;
5648 xq12_rw_in = 0.0;
5649 yq12_rw_in = 0.0;
5650 L12_rw_out = 1.0E+7;
5651 }
5652
5653 *A = yq12_rw_in - yq12_rw_out;
5654 *B = xq12_rw_out - xq12_rw_in;
5655 *C = xq12_rw_in * yq12_rw_out - xq12_rw_out * yq12_rw_in;
5656 }
5657
5658 /* 上面的结果依次是:圆1的切线xy坐标、角度,圆2的xy坐标、角度,两圆的距离,切线方程的A,B,C三个系数 */
5659 /* 圆2切入点的角度,与向的夹角 */
5660 fi1_rw_out_sum = ((6.2831853071795862 + *fis) - *fi1) / 6.2831853071795862;
5661 if (fabs(fi1_rw_out_sum - AFCS_MODEL1_rt_roundd_snf(fi1_rw_out_sum)) <=
5662 2.2204460492503131E-16 * fabs(fi1_rw_out_sum)) {
5663 fi1_rw_out_sum = 0.0;
5664 } else {
5665 fi1_rw_out_sum = (fi1_rw_out_sum - floor(fi1_rw_out_sum)) *
5666 6.2831853071795862;
5667 }
5668
5669 if (fabs(*fis - *fi1) < 1.0E-7) {
5670 fi1_rw_out_sum = 0.0;
5671
5672 /* 这块暂时没看懂 */
5673 }
5674
5675 *xq1 = xq12_rw_out;
5676 *yq1 = yq12_rw_out;
5677 *xq2 = xq12_rw_in;
5678 *yq2 = yq12_rw_in;
5679 fi2_rw_in_sum = ((6.2831853071795862 + *fi2) - *fif) / 6.2831853071795862;
5680 if (fabs(fi2_rw_in_sum - AFCS_MODEL1_rt_roundd_snf(fi2_rw_in_sum)) <=
5681 2.2204460492503131E-16 * fabs(fi2_rw_in_sum)) {
5682 fi2_rw_in_sum = 0.0;
5683 } else {
5684 fi2_rw_in_sum = (fi2_rw_in_sum - floor(fi2_rw_in_sum)) * 6.2831853071795862;
5685 }
5686
5687 if (fabs(*fi2 - *fif) < 1.0E-7) {
5688 fi2_rw_in_sum = 0.0;
5689 }
5690
5691 *L = (fi1_rw_out_sum * r_min + L12_rw_out) + fi2_rw_in_sum * r_min;
5692 if (fabs(v1x) + fabs(v1y) < 1.0E-7) {
5693 *xq1 = x1;
5694 *yq1 = b_y1;
5695 *fi1 = 0.0;
5696 }
5697
5698 if (fabs(v2x) + fabs(v2y) < 1.0E-7) {
5699 *xq2 = x2;
5700 *yq2 = y2;
5701 *fi2 = 0.0;
5702 }
5703}
5704
5705/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */
5706static void AFCS_MODEL1_PGT_circledot_b(real_T x, real_T y, real_T b_vx, real_T
5707 b_vy, real_T r, real_T *x_dot, real_T *y_dot, real_T *fi)
5708{
5709 real_T thet;
5710
5711 /* UNTITLED3 Summary of this function goes here */
5712 /* Detailed explanation goes here */
5713 /* YHT:求圆心坐标 */
5714 /* x,y,vx,vy:航点的位置坐标和速度 */
5715 /* r:圆的半径 */
5716 /* flag=1,表示顺时针,flag=0,表示逆时针 */
5717 if (b_vx >= 0.0) {
5718 if (fabs(b_vy) < 1.0E-7) {
5719 thet = 0.0;
5720 } else {
5721 thet = (atan(b_vy / b_vx) + 6.2831853071795862) / 6.2831853071795862;
5722 if (fabs(thet - AFCS_MODEL1_rt_roundd_snf(thet)) <= 2.2204460492503131E-16
5723 * fabs(thet)) {
5724 thet = 0.0;
5725 } else {
5726 thet = (thet - floor(thet)) * 6.2831853071795862;
5727 }
5728 }
5729 } else if (fabs(b_vy) < 1.0E-7) {
5730 thet = 3.1415926535897931;
5731 } else {
5732 thet = ((atan(b_vy / b_vx) + 3.1415926535897931) + 6.2831853071795862) /
5733 6.2831853071795862;
5734 if (fabs(thet - AFCS_MODEL1_rt_roundd_snf(thet)) <= 2.2204460492503131E-16 *
5735 fabs(thet)) {
5736 thet = 0.0;
5737 } else {
5738 thet = (thet - floor(thet)) * 6.2831853071795862;
5739 }
5740
5741 /* thet是切点处速度方向与北向的夹角,顺时针为正,0~360,yht */
5742 }
5743
5744 *x_dot = x - cos(thet - 1.5707963267948966) * r;
5745 *y_dot = y - sin(thet - 1.5707963267948966) * r;
5746 *fi = ((thet - 1.5707963267948966) + 6.2831853071795862) / 6.2831853071795862;
5747 if (fabs(*fi - AFCS_MODEL1_rt_roundd_snf(*fi)) <= 2.2204460492503131E-16 *
5748 fabs(*fi)) {
5749 *fi = 0.0;
5750 } else {
5751 *fi = (*fi - floor(*fi)) * 6.2831853071795862;
5752 }
5753
5754 /* 此值是什么意思? */
5755}
5756
5757/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */
5758static void AFCS_MODEL1_PGT_Dubins_2D_d(real_T x1, real_T b_y1, real_T v1x,
5759 real_T v1y, real_T x2, real_T y2, real_T v2x, real_T v2y, real_T r_min, real_T
5760 *xq1, real_T *yq1, real_T *fis, real_T *fi1, real_T *xq2, real_T *yq2, real_T *
5761 fif, real_T *fi2, real_T *L, real_T *A, real_T *B, real_T *C, real_T *x0_dot,
5762 real_T *y0_dot, real_T *tx_dot, real_T *ty_dot)
5763{
5764 static real_T fi2_rw_in_sum;
5765 static real_T b_x0_dot;
5766 static real_T b_y0_dot;
5767 static real_T b_fis;
5768 static real_T b_tx_dot;
5769 static real_T b_ty_dot;
5770 static real_T b_fif;
5771 static real_T fi1_rw_out_sum;
5772 static real_T xq12_rw_out;
5773 static real_T yq12_rw_out;
5774 static real_T xq12_rw_in;
5775 static real_T yq12_rw_in;
5776 static real_T L12_rw_out;
5777 static real_T c;
5778 static real_T afa;
5779
5780 /* UNTITLED 此处显示有关此函数的摘要 */
5781 /* 此处显示详细说明 */
5782 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
5783 /* xq1,yq1为圆1上切点的位置 */
5784 /* fis,fi1为圆1上起点和切出点的位置 */
5785 /* Lamda1表示圆1的方向,Lamda1=-1表示顺时针,Lamda1=1表示逆时针 */
5786 /* xq2,yq2为圆2上切点的位置 */
5787 /* fif,fi2为圆2上切入点和终点的位置 */
5788 /* Lamda2表示圆2的方向,Lamda2=-1表示顺时针,Lamda2=1表示逆时针 */
5789 /* L切线段长度。 */
5790 /* A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */
5791 /* x0_dot,y0_dot,tx_dot,ty_dot为圆1和圆2的圆心坐标 */
5792 /* x1,y1,v1x,v1y,x2,y2,v2x,v2y,r_min分别对应起始点位置,速度,终止点位置,速度 */
5793 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
5794 /* %求圆心坐标 */
5795 if (fabs(v1x) + fabs(v1y) < 1.0E-7) {
5796 *x0_dot = x1;
5797 *y0_dot = b_y1;
5798 *fis = 0.0;
5799 } else {
5800 AFCS_MODEL1_PGT_circledot(x1, b_y1, v1x, v1y, r_min, &b_x0_dot, &b_y0_dot,
5801 &b_fis);
5802 *x0_dot = b_x0_dot;
5803 *y0_dot = b_y0_dot;
5804 *fis = b_fis;
5805
5806 /* 起始圆圆心坐标,fis是圆1上的起点角度 */
5807 }
5808
5809 if (fabs(v2x) + fabs(v2y) < 1.0E-7) {
5810 *tx_dot = x2;
5811 *ty_dot = y2;
5812 *fif = 0.0;
5813 } else {
5814 AFCS_MODEL1_PGT_circledot_b(x2, y2, v2x, v2y, r_min, &b_tx_dot, &b_ty_dot,
5815 &b_fif);
5816 *tx_dot = b_tx_dot;
5817 *ty_dot = b_ty_dot;
5818 *fif = b_fif;
5819
5820 /* 目标圆圆心坐标 */
5821 }
5822
5823 /* %求路径长度及相应坐标 */
5824 /* UNTITLED2 Summary of this function goes here */
5825 /* 计算两圆之间的dubins路径长度,返回长度值和圆2的切入点位置 */
5826 /* 输入: */
5827 /* fis1: */
5828 /* x1,y1,r1:圆1的圆心坐标及半径 */
5829 /* flag1:圆1的转动方向标志 */
5830 /* x2,y2,r2:圆2的圆心坐标及半径 */
5831 /* flag2:圆2的转动方向标志 */
5832 /* 输出: */
5833 /* */
5834 /* Detailed explanation goes here */
5835 /* UNTITLED2 Summary of this function goes here */
5836 /* Detailed explanation goes here */
5837 /* flag1=0,表示圆1在切线右边,flag1=1,表示圆1在切线左边; */
5838 /* flag2=0,表示外切线,flag2=1,表示内切线。 */
5839 /* A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */
5840 /* L切线段长度。 */
5841 /* fi1表示圆1上切点的位置,fi2表示圆2上切点的位置。 */
5842 /* 输入: */
5843 /* x1,y1,r1:圆1的圆心坐标及半径 */
5844 /* flag1:圆1的转动方向标志 */
5845 /* x2,y2,r2:圆2的圆心坐标及半径 */
5846 /* flag2:圆2的转动方向标志 */
5847 if ((*x0_dot == *tx_dot) && (*y0_dot == *ty_dot)) {
5848 /* 如果圆1和圆2重合 */
5849 xq12_rw_out = 0.0;
5850 yq12_rw_out = 0.0;
5851 *fi1 = 0.0;
5852 xq12_rw_in = 0.0;
5853 yq12_rw_in = 0.0;
5854 *fi2 = 0.0;
5855 L12_rw_out = 1.0E+7;
5856 *A = 1.0;
5857 *B = 1.0;
5858 *C = 1.0;
5859 } else {
5860 c = *x0_dot - *tx_dot;
5861 *fi1 = *y0_dot - *ty_dot;
5862 c = sqrt(c * c + *fi1 * *fi1);
5863
5864 /* 两圆的距离 */
5865 /* 外切线存在条件d_w<0; */
5866 /* 内切线存在条件d_n<0; */
5867 /* 圆在切线的右侧 */
5868 if (fabs(r_min + r_min) - c < 0.0) {
5869 /* 内切 */
5870 afa = acos((r_min + r_min) / c);
5871 *fi2 = AFCS_MODEL1_rt_atan2d_snf(*ty_dot - *y0_dot, *tx_dot - *x0_dot);
5872 *fi1 = ((afa + *fi2) + 6.2831853071795862) / 6.2831853071795862;
5873 if (fabs(*fi1 - AFCS_MODEL1_rt_roundd_snf(*fi1)) <= 2.2204460492503131E-16
5874 * fabs(*fi1)) {
5875 *fi1 = 0.0;
5876 } else {
5877 *fi1 = (*fi1 - floor(*fi1)) * 6.2831853071795862;
5878 }
5879
5880 *fi2 = (((afa + *fi2) + 3.1415926535897931) + 6.2831853071795862) /
5881 6.2831853071795862;
5882 if (fabs(*fi2 - AFCS_MODEL1_rt_roundd_snf(*fi2)) <= 2.2204460492503131E-16
5883 * fabs(*fi2)) {
5884 *fi2 = 0.0;
5885 } else {
5886 *fi2 = (*fi2 - floor(*fi2)) * 6.2831853071795862;
5887 }
5888
5889 xq12_rw_out = r_min * cos(*fi1) + *x0_dot;
5890 yq12_rw_out = r_min * sin(*fi1) + *y0_dot;
5891 xq12_rw_in = r_min * cos(*fi2) + *tx_dot;
5892 yq12_rw_in = r_min * sin(*fi2) + *ty_dot;
5893 L12_rw_out = xq12_rw_out - xq12_rw_in;
5894 fi1_rw_out_sum = yq12_rw_out - yq12_rw_in;
5895 L12_rw_out = sqrt(L12_rw_out * L12_rw_out + fi1_rw_out_sum *
5896 fi1_rw_out_sum);
5897 } else {
5898 *fi1 = 0.0;
5899 *fi2 = 0.0;
5900 xq12_rw_out = 0.0;
5901 yq12_rw_out = 0.0;
5902 xq12_rw_in = 0.0;
5903 yq12_rw_in = 0.0;
5904 L12_rw_out = 1.0E+7;
5905 }
5906
5907 *A = yq12_rw_in - yq12_rw_out;
5908 *B = xq12_rw_out - xq12_rw_in;
5909 *C = xq12_rw_in * yq12_rw_out - xq12_rw_out * yq12_rw_in;
5910 }
5911
5912 /* 上面的结果依次是:圆1的切线xy坐标、角度,圆2的xy坐标、角度,两圆的距离,切线方程的A,B,C三个系数 */
5913 /* 圆2切入点的角度,与向的夹角 */
5914 fi1_rw_out_sum = ((6.2831853071795862 + *fis) - *fi1) / 6.2831853071795862;
5915 if (fabs(fi1_rw_out_sum - AFCS_MODEL1_rt_roundd_snf(fi1_rw_out_sum)) <=
5916 2.2204460492503131E-16 * fabs(fi1_rw_out_sum)) {
5917 fi1_rw_out_sum = 0.0;
5918 } else {
5919 fi1_rw_out_sum = (fi1_rw_out_sum - floor(fi1_rw_out_sum)) *
5920 6.2831853071795862;
5921 }
5922
5923 if (fabs(*fis - *fi1) < 1.0E-7) {
5924 fi1_rw_out_sum = 0.0;
5925
5926 /* 这块暂时没看懂 */
5927 }
5928
5929 *xq1 = xq12_rw_out;
5930 *yq1 = yq12_rw_out;
5931 *xq2 = xq12_rw_in;
5932 *yq2 = yq12_rw_in;
5933 fi2_rw_in_sum = ((6.2831853071795862 + *fif) - *fi2) / 6.2831853071795862;
5934 if (fabs(fi2_rw_in_sum - AFCS_MODEL1_rt_roundd_snf(fi2_rw_in_sum)) <=
5935 2.2204460492503131E-16 * fabs(fi2_rw_in_sum)) {
5936 fi2_rw_in_sum = 0.0;
5937 } else {
5938 fi2_rw_in_sum = (fi2_rw_in_sum - floor(fi2_rw_in_sum)) * 6.2831853071795862;
5939 }
5940
5941 if (fabs(*fi2 - *fif) < 1.0E-7) {
5942 fi2_rw_in_sum = 0.0;
5943 }
5944
5945 *L = (fi1_rw_out_sum * r_min + L12_rw_out) + fi2_rw_in_sum * r_min;
5946 if (fabs(v1x) + fabs(v1y) < 1.0E-7) {
5947 *xq1 = x1;
5948 *yq1 = b_y1;
5949 *fi1 = 0.0;
5950 }
5951
5952 if (fabs(v2x) + fabs(v2y) < 1.0E-7) {
5953 *xq2 = x2;
5954 *yq2 = y2;
5955 *fi2 = 0.0;
5956 }
5957}
5958
5959/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */
5960static void AFCS_MODEL1_PGT_Dubins_2Dmin(real_T x1, real_T b_y1, real_T v1x,
5961 real_T v1y, real_T x2, real_T y2, real_T v2x, real_T v2y, real_T r_min, real_T
5962 *xq1, real_T *yq1, real_T *fis, real_T *fi1, real_T *Lamda1, real_T *xq2,
5963 real_T *yq2, real_T *fif, real_T *fi2, real_T *Lamda2, real_T *L, real_T
5964 *x0_dot, real_T *y0_dot, real_T *tx_dot, real_T *ty_dot)
5965{
5966 static real_T xq1_1;
5967 static real_T yq1_1;
5968 static real_T fis_1;
5969 static real_T fi1_1;
5970 static real_T xq2_1;
5971 static real_T yq2_1;
5972 static real_T fif_1;
5973 static real_T fi2_1;
5974 static real_T L_1;
5975 static real_T unusedU0;
5976 static real_T unusedU1;
5977 static real_T unusedU2;
5978 static real_T x0_dot_1;
5979 static real_T y0_dot_1;
5980 static real_T tx_dot_1;
5981 static real_T ty_dot_1;
5982 static real_T xq1_2;
5983 static real_T yq1_2;
5984 static real_T fis_2;
5985 static real_T fi1_2;
5986 static real_T xq2_2;
5987 static real_T yq2_2;
5988 static real_T fif_2;
5989 static real_T fi2_2;
5990 static real_T L_2;
5991 static real_T unusedU3;
5992 static real_T unusedU4;
5993 static real_T unusedU5;
5994 static real_T x0_dot_2;
5995 static real_T y0_dot_2;
5996 static real_T tx_dot_2;
5997 static real_T ty_dot_2;
5998 static real_T xq1_3;
5999 static real_T yq1_3;
6000 static real_T fis_3;
6001 static real_T fi1_3;
6002 static real_T xq2_3;
6003 static real_T yq2_3;
6004 static real_T fif_3;
6005 static real_T fi2_3;
6006 static real_T L_3;
6007 static real_T x0_dot_3;
6008 static real_T y0_dot_3;
6009 static real_T tx_dot_3;
6010 static real_T ty_dot_3;
6011 static real_T xq1_4;
6012 static real_T yq1_4;
6013 static real_T fis_4;
6014 static real_T fi1_4;
6015 static real_T xq2_4;
6016 static real_T yq2_4;
6017 static real_T fif_4;
6018 static real_T fi2_4;
6019 static real_T L_4;
6020 static real_T x0_dot_4;
6021 static real_T y0_dot_4;
6022 static real_T tx_dot_4;
6023 static real_T ty_dot_4;
6024 static real_T fi2_rw_in_sum;
6025 static real_T b_x0_dot;
6026 static real_T b_y0_dot;
6027 static real_T b_fis;
6028 static real_T b_tx_dot;
6029 static real_T b_ty_dot;
6030 static real_T b_fif;
6031 static real_T fi1_rw_out_sum;
6032 static real_T xq12_rw_out;
6033 static real_T yq12_rw_out;
6034 static real_T fi12_rw_out;
6035 static real_T xq12_rw_in;
6036 static real_T yq12_rw_in;
6037 static real_T fi12_rw_in;
6038 static real_T L12_rw_out;
6039 static real_T c;
6040 static real_T afa;
6041 static real_T belt;
6042 static real_T a;
6043 static real_T b_a;
6044 static real_T c_a;
6045 static real_T d_a;
6046 static real_T b_fi2_rw_in_sum;
6047 static real_T c_x0_dot;
6048 static real_T c_y0_dot;
6049 static real_T c_fis;
6050 static real_T c_tx_dot;
6051 static real_T c_ty_dot;
6052 static real_T c_fif;
6053 static real_T b_fi1_rw_out_sum;
6054 static real_T b_xq12_rw_out;
6055 static real_T b_yq12_rw_out;
6056 static real_T b_fi12_rw_out;
6057 static real_T b_xq12_rw_in;
6058 static real_T b_yq12_rw_in;
6059 static real_T b_fi12_rw_in;
6060 static real_T b_L12_rw_out;
6061 static real_T b_c;
6062 static real_T b_afa;
6063 static real_T b_belt;
6064 static real_T e_a;
6065 static real_T f_a;
6066 static real_T g_a;
6067 static real_T h_a;
6068
6069 /* UNTITLED2 此处显示有关此函数的摘要 */
6070 /* 此处显示详细说明 */
6071 /* %1 */
6072 AFCS_MODEL1_PGT_Dubins_2D(x1, b_y1, v1x, v1y, x2, y2, v2x, v2y, r_min, &xq1_1,
6073 &yq1_1, &fis_1, &fi1_1, &xq2_1, &yq2_1, &fif_1, &fi2_1, &L_1, &unusedU0,
6074 &unusedU1, &unusedU2, &x0_dot_1, &y0_dot_1, &tx_dot_1, &ty_dot_1);
6075 *xq1 = xq1_1;
6076 *yq1 = yq1_1;
6077 *fis = fis_1;
6078 *fi1 = fi1_1;
6079 *Lamda1 = 1.0;
6080 *xq2 = xq2_1;
6081 *yq2 = yq2_1;
6082 *fif = fif_1;
6083 *fi2 = fi2_1;
6084 *Lamda2 = 1.0;
6085 *L = L_1;
6086 *x0_dot = x0_dot_1;
6087 *y0_dot = y0_dot_1;
6088 *tx_dot = tx_dot_1;
6089 *ty_dot = ty_dot_1;
6090
6091 /* %2 */
6092 AFCS_MODEL1_PGT_Dubins_2D_d(x1, b_y1, v1x, v1y, x2, y2, v2x, v2y, r_min,
6093 &xq1_2, &yq1_2, &fis_2, &fi1_2, &xq2_2, &yq2_2, &fif_2, &fi2_2, &L_2,
6094 &unusedU3, &unusedU4, &unusedU5, &x0_dot_2, &y0_dot_2, &tx_dot_2, &ty_dot_2);
6095 if (L_2 <= L_1 + 1.0E-7) {
6096 *xq1 = xq1_2;
6097 *yq1 = yq1_2;
6098 *fis = fis_2;
6099 *fi1 = fi1_2;
6100 *xq2 = xq2_2;
6101 *yq2 = yq2_2;
6102 *fif = fif_2;
6103 *fi2 = fi2_2;
6104 *Lamda2 = -1.0;
6105 *L = L_2;
6106 *x0_dot = x0_dot_2;
6107 *y0_dot = y0_dot_2;
6108 *tx_dot = tx_dot_2;
6109 *ty_dot = ty_dot_2;
6110 }
6111
6112 /* %3 */
6113 /* UNTITLED 此处显示有关此函数的摘要 */
6114 /* 此处显示详细说明 */
6115 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
6116 /* xq1,yq1为圆1上切点的位置 */
6117 /* fis,fi1为圆1上起点和切出点的位置 */
6118 /* Lamda1表示圆1的方向,Lamda1=-1表示顺时针,Lamda1=1表示逆时针 */
6119 /* xq2,yq2为圆2上切点的位置 */
6120 /* fif,fi2为圆2上切入点和终点的位置 */
6121 /* Lamda2表示圆2的方向,Lamda2=-1表示顺时针,Lamda2=1表示逆时针 */
6122 /* L切线段长度。 */
6123 /* A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */
6124 /* x0_dot,y0_dot,tx_dot,ty_dot为圆1和圆2的圆心坐标 */
6125 /* x1,y1,v1x,v1y,x2,y2,v2x,v2y,r_min分别对应起始点位置,速度,终止点位置,速度 */
6126 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
6127 /* %求圆心坐标 */
6128 if (fabs(v1x) + fabs(v1y) < 1.0E-7) {
6129 x0_dot_3 = x1;
6130 y0_dot_3 = b_y1;
6131 fis_3 = 0.0;
6132 } else {
6133 AFCS_MODEL1_PGT_circledot_b(x1, b_y1, v1x, v1y, r_min, &b_x0_dot, &b_y0_dot,
6134 &b_fis);
6135 x0_dot_3 = b_x0_dot;
6136 y0_dot_3 = b_y0_dot;
6137 fis_3 = b_fis;
6138
6139 /* 起始圆圆心坐标,fis是圆1上的起点角度 */
6140 }
6141
6142 if (fabs(v2x) + fabs(v2y) < 1.0E-7) {
6143 tx_dot_3 = x2;
6144 ty_dot_3 = y2;
6145 fif_3 = 0.0;
6146 } else {
6147 AFCS_MODEL1_PGT_circledot(x2, y2, v2x, v2y, r_min, &b_tx_dot, &b_ty_dot,
6148 &b_fif);
6149 tx_dot_3 = b_tx_dot;
6150 ty_dot_3 = b_ty_dot;
6151 fif_3 = b_fif;
6152
6153 /* 目标圆圆心坐标 */
6154 }
6155
6156 /* 逆时针为1,顺时针为-1 */
6157 /* %求路径长度及相应坐标 */
6158 /* UNTITLED2 Summary of this function goes here */
6159 /* 计算两圆之间的dubins路径长度,返回长度值和圆2的切入点位置 */
6160 /* 输入: */
6161 /* fis1: */
6162 /* x1,y1,r1:圆1的圆心坐标及半径 */
6163 /* flag1:圆1的转动方向标志 */
6164 /* x2,y2,r2:圆2的圆心坐标及半径 */
6165 /* flag2:圆2的转动方向标志 */
6166 /* 输出: */
6167 /* */
6168 /* Detailed explanation goes here */
6169 /* UNTITLED2 Summary of this function goes here */
6170 /* Detailed explanation goes here */
6171 /* flag1=0,表示圆1在切线右边,flag1=1,表示圆1在切线左边; */
6172 /* flag2=0,表示外切线,flag2=1,表示内切线。 */
6173 /* A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */
6174 /* L切线段长度。 */
6175 /* fi1表示圆1上切点的位置,fi2表示圆2上切点的位置。 */
6176 /* 输入: */
6177 /* x1,y1,r1:圆1的圆心坐标及半径 */
6178 /* flag1:圆1的转动方向标志 */
6179 /* x2,y2,r2:圆2的圆心坐标及半径 */
6180 /* flag2:圆2的转动方向标志 */
6181 if ((x0_dot_3 == tx_dot_3) && (y0_dot_3 == ty_dot_3)) {
6182 /* 如果圆1和圆2重合 */
6183 xq12_rw_out = 0.0;
6184 yq12_rw_out = 0.0;
6185 fi12_rw_out = 0.0;
6186 xq12_rw_in = 0.0;
6187 yq12_rw_in = 0.0;
6188 fi12_rw_in = 0.0;
6189 L12_rw_out = 1.0E+7;
6190 } else {
6191 a = x0_dot_3 - tx_dot_3;
6192 b_a = y0_dot_3 - ty_dot_3;
6193 c = sqrt(a * a + b_a * b_a);
6194
6195 /* 两圆的距离 */
6196 /* 外切线存在条件d_w<0; */
6197 /* 内切线存在条件d_n<0; */
6198 /* 圆在切线的左侧 */
6199 if (fabs(r_min + r_min) - c < 0.0) {
6200 /* 内切 */
6201 afa = acos((r_min + r_min) / c);
6202 belt = AFCS_MODEL1_rt_atan2d_snf(ty_dot_3 - y0_dot_3, tx_dot_3 - x0_dot_3);
6203 fi12_rw_out = (((-afa + belt) + 6.2831853071795862) + 6.2831853071795862) /
6204 6.2831853071795862;
6205 if (fabs(fi12_rw_out - AFCS_MODEL1_rt_roundd_snf(fi12_rw_out)) <=
6206 2.2204460492503131E-16 * fabs(fi12_rw_out)) {
6207 fi12_rw_out = 0.0;
6208 } else {
6209 fi12_rw_out = (fi12_rw_out - floor(fi12_rw_out)) * 6.2831853071795862;
6210 }
6211
6212 fi12_rw_in = (((-afa + belt) + 3.1415926535897931) + 6.2831853071795862) /
6213 6.2831853071795862;
6214 if (fabs(fi12_rw_in - AFCS_MODEL1_rt_roundd_snf(fi12_rw_in)) <=
6215 2.2204460492503131E-16 * fabs(fi12_rw_in)) {
6216 fi12_rw_in = 0.0;
6217 } else {
6218 fi12_rw_in = (fi12_rw_in - floor(fi12_rw_in)) * 6.2831853071795862;
6219 }
6220
6221 xq12_rw_out = r_min * cos(fi12_rw_out) + x0_dot_3;
6222 yq12_rw_out = r_min * sin(fi12_rw_out) + y0_dot_3;
6223 xq12_rw_in = r_min * cos(fi12_rw_in) + tx_dot_3;
6224 yq12_rw_in = r_min * sin(fi12_rw_in) + ty_dot_3;
6225 c_a = xq12_rw_out - xq12_rw_in;
6226 d_a = yq12_rw_out - yq12_rw_in;
6227 L12_rw_out = sqrt(c_a * c_a + d_a * d_a);
6228 } else {
6229 fi12_rw_out = 0.0;
6230 fi12_rw_in = 0.0;
6231 xq12_rw_out = 0.0;
6232 yq12_rw_out = 0.0;
6233 xq12_rw_in = 0.0;
6234 yq12_rw_in = 0.0;
6235 L12_rw_out = 1.0E+7;
6236 }
6237 }
6238
6239 /* 上面的结果依次是:圆1的切线xy坐标、角度,圆2的xy坐标、角度,两圆的距离,切线方程的A,B,C三个系数 */
6240 /* 圆2切入点的角度,与向的夹角 */
6241 fi1_rw_out_sum = ((6.2831853071795862 + fi12_rw_out) - fis_3) /
6242 6.2831853071795862;
6243 if (fabs(fi1_rw_out_sum - AFCS_MODEL1_rt_roundd_snf(fi1_rw_out_sum)) <=
6244 2.2204460492503131E-16 * fabs(fi1_rw_out_sum)) {
6245 fi1_rw_out_sum = 0.0;
6246 } else {
6247 fi1_rw_out_sum = (fi1_rw_out_sum - floor(fi1_rw_out_sum)) *
6248 6.2831853071795862;
6249 }
6250
6251 if (fabs(fis_3 - fi12_rw_out) < 1.0E-7) {
6252 fi1_rw_out_sum = 0.0;
6253
6254 /* 这块暂时没看懂 */
6255 }
6256
6257 xq1_3 = xq12_rw_out;
6258 yq1_3 = yq12_rw_out;
6259 fi1_3 = fi12_rw_out;
6260 xq2_3 = xq12_rw_in;
6261 yq2_3 = yq12_rw_in;
6262 fi2_3 = fi12_rw_in;
6263 fi2_rw_in_sum = ((6.2831853071795862 + fi12_rw_in) - fif_3) /
6264 6.2831853071795862;
6265 if (fabs(fi2_rw_in_sum - AFCS_MODEL1_rt_roundd_snf(fi2_rw_in_sum)) <=
6266 2.2204460492503131E-16 * fabs(fi2_rw_in_sum)) {
6267 fi2_rw_in_sum = 0.0;
6268 } else {
6269 fi2_rw_in_sum = (fi2_rw_in_sum - floor(fi2_rw_in_sum)) * 6.2831853071795862;
6270 }
6271
6272 if (fabs(fi12_rw_in - fif_3) < 1.0E-7) {
6273 fi2_rw_in_sum = 0.0;
6274 }
6275
6276 L_3 = (fi1_rw_out_sum * r_min + L12_rw_out) + fi2_rw_in_sum * r_min;
6277 if (fabs(v1x) + fabs(v1y) < 1.0E-7) {
6278 xq1_3 = x1;
6279 yq1_3 = b_y1;
6280 fi1_3 = 0.0;
6281 }
6282
6283 if (fabs(v2x) + fabs(v2y) < 1.0E-7) {
6284 xq2_3 = x2;
6285 yq2_3 = y2;
6286 fi2_3 = 0.0;
6287 }
6288
6289 if (L_3 <= *L + 1.0E-7) {
6290 *xq1 = xq1_3;
6291 *yq1 = yq1_3;
6292 *fis = fis_3;
6293 *fi1 = fi1_3;
6294 *Lamda1 = -1.0;
6295 *xq2 = xq2_3;
6296 *yq2 = yq2_3;
6297 *fif = fif_3;
6298 *fi2 = fi2_3;
6299 *Lamda2 = 1.0;
6300 *L = L_3;
6301 *x0_dot = x0_dot_3;
6302 *y0_dot = y0_dot_3;
6303 *tx_dot = tx_dot_3;
6304 *ty_dot = ty_dot_3;
6305 }
6306
6307 /* %4 */
6308 /* UNTITLED 此处显示有关此函数的摘要 */
6309 /* 此处显示详细说明 */
6310 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
6311 /* xq1,yq1为圆1上切点的位置 */
6312 /* fis,fi1为圆1上起点和切出点的位置 */
6313 /* Lamda1表示圆1的方向,Lamda1=-1表示顺时针,Lamda1=1表示逆时针 */
6314 /* xq2,yq2为圆2上切点的位置 */
6315 /* fif,fi2为圆2上切入点和终点的位置 */
6316 /* Lamda2表示圆2的方向,Lamda2=-1表示顺时针,Lamda2=1表示逆时针 */
6317 /* L切线段长度。 */
6318 /* A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */
6319 /* x0_dot,y0_dot,tx_dot,ty_dot为圆1和圆2的圆心坐标 */
6320 /* x1,y1,v1x,v1y,x2,y2,v2x,v2y,r_min分别对应起始点位置,速度,终止点位置,速度 */
6321 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
6322 /* %求圆心坐标 */
6323 if (fabs(v1x) + fabs(v1y) < 1.0E-7) {
6324 x0_dot_4 = x1;
6325 y0_dot_4 = b_y1;
6326 fis_4 = 0.0;
6327 } else {
6328 AFCS_MODEL1_PGT_circledot_b(x1, b_y1, v1x, v1y, r_min, &c_x0_dot, &c_y0_dot,
6329 &c_fis);
6330 x0_dot_4 = c_x0_dot;
6331 y0_dot_4 = c_y0_dot;
6332 fis_4 = c_fis;
6333
6334 /* 起始圆圆心坐标,fis是圆1上的起点角度 */
6335 }
6336
6337 if (fabs(v2x) + fabs(v2y) < 1.0E-7) {
6338 tx_dot_4 = x2;
6339 ty_dot_4 = y2;
6340 fif_4 = 0.0;
6341 } else {
6342 AFCS_MODEL1_PGT_circledot_b(x2, y2, v2x, v2y, r_min, &c_tx_dot, &c_ty_dot,
6343 &c_fif);
6344 tx_dot_4 = c_tx_dot;
6345 ty_dot_4 = c_ty_dot;
6346 fif_4 = c_fif;
6347
6348 /* 目标圆圆心坐标 */
6349 }
6350
6351 /* 逆时针为1,顺时针为-1 */
6352 /* %求路径长度及相应坐标 */
6353 /* UNTITLED2 Summary of this function goes here */
6354 /* 计算两圆之间的dubins路径长度,返回长度值和圆2的切入点位置 */
6355 /* 输入: */
6356 /* fis1: */
6357 /* x1,y1,r1:圆1的圆心坐标及半径 */
6358 /* flag1:圆1的转动方向标志 */
6359 /* x2,y2,r2:圆2的圆心坐标及半径 */
6360 /* flag2:圆2的转动方向标志 */
6361 /* 输出: */
6362 /* */
6363 /* Detailed explanation goes here */
6364 /* UNTITLED2 Summary of this function goes here */
6365 /* Detailed explanation goes here */
6366 /* flag1=0,表示圆1在切线右边,flag1=1,表示圆1在切线左边; */
6367 /* flag2=0,表示外切线,flag2=1,表示内切线。 */
6368 /* A,B,C为连接两切点的直线系数。切线方程为Ax+By+C=0. */
6369 /* L切线段长度。 */
6370 /* fi1表示圆1上切点的位置,fi2表示圆2上切点的位置。 */
6371 /* 输入: */
6372 /* x1,y1,r1:圆1的圆心坐标及半径 */
6373 /* flag1:圆1的转动方向标志 */
6374 /* x2,y2,r2:圆2的圆心坐标及半径 */
6375 /* flag2:圆2的转动方向标志 */
6376 if ((x0_dot_4 == tx_dot_4) && (y0_dot_4 == ty_dot_4)) {
6377 /* 如果圆1和圆2重合 */
6378 b_xq12_rw_out = 0.0;
6379 b_yq12_rw_out = 0.0;
6380 b_fi12_rw_out = 0.0;
6381 b_xq12_rw_in = 0.0;
6382 b_yq12_rw_in = 0.0;
6383 b_fi12_rw_in = 0.0;
6384 b_L12_rw_out = 1.0E+7;
6385 } else {
6386 e_a = x0_dot_4 - tx_dot_4;
6387 f_a = y0_dot_4 - ty_dot_4;
6388 b_c = sqrt(e_a * e_a + f_a * f_a);
6389
6390 /* 两圆的距离 */
6391 /* 外切线存在条件d_w<0; */
6392 /* 内切线存在条件d_n<0; */
6393 /* 圆在切线的左侧 */
6394 if (fabs(r_min - r_min) - b_c < 0.0) {
6395 /* 外切 */
6396 b_afa = asin((r_min - r_min) / b_c);
6397 b_belt = AFCS_MODEL1_rt_atan2d_snf(ty_dot_4 - y0_dot_4, tx_dot_4 -
6398 x0_dot_4);
6399 b_fi12_rw_out = (((-b_afa + b_belt) + 4.71238898038469) +
6400 6.2831853071795862) / 6.2831853071795862;
6401 if (fabs(b_fi12_rw_out - AFCS_MODEL1_rt_roundd_snf(b_fi12_rw_out)) <=
6402 2.2204460492503131E-16 * fabs(b_fi12_rw_out)) {
6403 b_fi12_rw_out = 0.0;
6404 } else {
6405 b_fi12_rw_out = (b_fi12_rw_out - floor(b_fi12_rw_out)) *
6406 6.2831853071795862;
6407 }
6408
6409 b_fi12_rw_in = (((-b_afa + b_belt) + 4.71238898038469) +
6410 6.2831853071795862) / 6.2831853071795862;
6411 if (fabs(b_fi12_rw_in - AFCS_MODEL1_rt_roundd_snf(b_fi12_rw_in)) <=
6412 2.2204460492503131E-16 * fabs(b_fi12_rw_in)) {
6413 b_fi12_rw_in = 0.0;
6414 } else {
6415 b_fi12_rw_in = (b_fi12_rw_in - floor(b_fi12_rw_in)) * 6.2831853071795862;
6416 }
6417
6418 b_xq12_rw_out = r_min * cos(b_fi12_rw_out) + x0_dot_4;
6419 b_yq12_rw_out = r_min * sin(b_fi12_rw_out) + y0_dot_4;
6420 b_xq12_rw_in = r_min * cos(b_fi12_rw_in) + tx_dot_4;
6421 b_yq12_rw_in = r_min * sin(b_fi12_rw_in) + ty_dot_4;
6422 g_a = b_xq12_rw_out - b_xq12_rw_in;
6423 h_a = b_yq12_rw_out - b_yq12_rw_in;
6424 b_L12_rw_out = sqrt(g_a * g_a + h_a * h_a);
6425 } else {
6426 b_fi12_rw_out = 0.0;
6427 b_fi12_rw_in = 0.0;
6428 b_xq12_rw_out = 0.0;
6429 b_yq12_rw_out = 0.0;
6430 b_xq12_rw_in = 0.0;
6431 b_yq12_rw_in = 0.0;
6432 b_L12_rw_out = 1.0E+7;
6433 }
6434 }
6435
6436 /* 上面的结果依次是:圆1的切线xy坐标、角度,圆2的xy坐标、角度,两圆的距离,切线方程的A,B,C三个系数 */
6437 /* 圆2切入点的角度,与向的夹角 */
6438 b_fi1_rw_out_sum = ((6.2831853071795862 + b_fi12_rw_out) - fis_4) /
6439 6.2831853071795862;
6440 if (fabs(b_fi1_rw_out_sum - AFCS_MODEL1_rt_roundd_snf(b_fi1_rw_out_sum)) <=
6441 2.2204460492503131E-16 * fabs(b_fi1_rw_out_sum)) {
6442 b_fi1_rw_out_sum = 0.0;
6443 } else {
6444 b_fi1_rw_out_sum = (b_fi1_rw_out_sum - floor(b_fi1_rw_out_sum)) *
6445 6.2831853071795862;
6446 }
6447
6448 if (fabs(fis_4 - b_fi12_rw_out) < 1.0E-7) {
6449 b_fi1_rw_out_sum = 0.0;
6450
6451 /* 这块暂时没看懂 */
6452 }
6453
6454 xq1_4 = b_xq12_rw_out;
6455 yq1_4 = b_yq12_rw_out;
6456 fi1_4 = b_fi12_rw_out;
6457 xq2_4 = b_xq12_rw_in;
6458 yq2_4 = b_yq12_rw_in;
6459 fi2_4 = b_fi12_rw_in;
6460 b_fi2_rw_in_sum = ((6.2831853071795862 + fif_4) - b_fi12_rw_in) /
6461 6.2831853071795862;
6462 if (fabs(b_fi2_rw_in_sum - AFCS_MODEL1_rt_roundd_snf(b_fi2_rw_in_sum)) <=
6463 2.2204460492503131E-16 * fabs(b_fi2_rw_in_sum)) {
6464 b_fi2_rw_in_sum = 0.0;
6465 } else {
6466 b_fi2_rw_in_sum = (b_fi2_rw_in_sum - floor(b_fi2_rw_in_sum)) *
6467 6.2831853071795862;
6468 }
6469
6470 if (fabs(b_fi12_rw_in - fif_4) < 1.0E-7) {
6471 b_fi2_rw_in_sum = 0.0;
6472 }
6473
6474 L_4 = (b_fi1_rw_out_sum * r_min + b_L12_rw_out) + b_fi2_rw_in_sum * r_min;
6475 if (fabs(v1x) + fabs(v1y) < 1.0E-7) {
6476 xq1_4 = x1;
6477 yq1_4 = b_y1;
6478 fi1_4 = 0.0;
6479 }
6480
6481 if (fabs(v2x) + fabs(v2y) < 1.0E-7) {
6482 xq2_4 = x2;
6483 yq2_4 = y2;
6484 fi2_4 = 0.0;
6485 }
6486
6487 if (L_4 <= *L + 1.0E-7) {
6488 *xq1 = xq1_4;
6489 *yq1 = yq1_4;
6490 *fis = fis_4;
6491 *fi1 = fi1_4;
6492 *Lamda1 = -1.0;
6493 *xq2 = xq2_4;
6494 *yq2 = yq2_4;
6495 *fif = fif_4;
6496 *fi2 = fi2_4;
6497 *Lamda2 = -1.0;
6498 *L = L_4;
6499 *x0_dot = x0_dot_4;
6500 *y0_dot = y0_dot_4;
6501 *tx_dot = tx_dot_4;
6502 *ty_dot = ty_dot_4;
6503 }
6504
6505 /* */
6506 /* 画圆 */
6507 /* % % if plotflag */
6508 /* % figure(2);subplot(2,2,1);plot([yq1_1,yq2_1],[xq1_1,xq2_1],'LineStyle','--','Marker','o','LineWidth',2,'color','r');hold on */
6509 /* % plot([y1,y2],[x1,x2],'LineStyle','none','Marker','o','LineWidth',4); axis equal */
6510 /* % rectangle('Position',[y0_dot_1- r_min,x0_dot_1-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','g') */
6511 /* % rectangle('Position',[ty_dot_1- r_min,tx_dot_1-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','b') */
6512 /* % %画图 */
6513 /* % figure(2);subplot(2,2,2);plot([yq1_2,yq2_2],[xq1_2,xq2_2],'LineStyle','--','Marker','o','LineWidth',2,'color','r');hold on */
6514 /* % plot([y1,y2],[x1,x2],'LineStyle','none','Marker','o','LineWidth',4); axis equal */
6515 /* % rectangle('Position',[y0_dot_2- r_min,x0_dot_2-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','g') */
6516 /* % rectangle('Position',[ty_dot_2- r_min,tx_dot_2-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','b') */
6517 /* % %画图 */
6518 /* % figure(2);subplot(2,2,3);plot([yq1_3,yq2_3],[xq1_3,xq2_3],'LineStyle','--','Marker','o','LineWidth',2,'color','r');hold on */
6519 /* % plot([y1,y2],[x1,x2],'LineStyle','none','Marker','o','LineWidth',4); axis equal */
6520 /* % rectangle('Position',[y0_dot_3- r_min,x0_dot_3-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','g') */
6521 /* % rectangle('Position',[ty_dot_3- r_min,tx_dot_3-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','b') */
6522 /* % %画图 */
6523 /* % figure(2);subplot(2,2,4);plot([yq1_4,yq2_4],[xq1_3,xq2_4],'LineStyle','--','Marker','o','LineWidth',2,'color','r');hold on */
6524 /* % plot([y1,y2],[x1,x2],'LineStyle','none','Marker','o','LineWidth',4); axis equal */
6525 /* % rectangle('Position',[y0_dot_4- r_min,x0_dot_4-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','g') */
6526 /* % rectangle('Position',[ty_dot_4- r_min,tx_dot_4-r_min,2*r_min,2*r_min],'Curvature',[1,1],'EdgeColor','b') */
6527 /* end */
6528}
6529
6530/* Function for MATLAB Function: '<S335>/UAV Trajectory Planning' */
6531static void AFCS_MODE_PGT_Guidance3D_Dubins(real_T xs, real_T ys, real_T hs,
6532 real_T vxs, real_T vys, real_T xe, real_T ye, real_T he, real_T vxe, real_T
6533 vye, real_T Rmin, real_T *R, real_T *Gama, real_T *xq1, real_T *yq1, real_T
6534 *hq1, real_T *fis, real_T *Lamda1, real_T *xq2, real_T *yq2, real_T *hq2,
6535 real_T *fi2, real_T *Lamda2, real_T *x0_dot, real_T *y0_dot, real_T *tx_dot,
6536 real_T *ty_dot, real_T *fi1sum, real_T *fi2sum)
6537{
6538 static real_T fi1;
6539 static real_T fif;
6540 static real_T L;
6541 int32_T count;
6542 static real_T b_xq1;
6543 static real_T b_yq1;
6544 static real_T b_fis;
6545 static real_T b_fi1;
6546 static real_T b_Lamda1;
6547 static real_T b_xq2;
6548 static real_T b_yq2;
6549 static real_T b_fif;
6550 static real_T b_fi2;
6551 static real_T b_Lamda2;
6552 static real_T b_L;
6553 static real_T b_x0_dot;
6554 static real_T b_y0_dot;
6555 static real_T b_tx_dot;
6556 static real_T b_ty_dot;
6557 static real_T c_xq1;
6558 static real_T c_yq1;
6559 static real_T c_fis;
6560 static real_T c_fi1;
6561 static real_T c_Lamda1;
6562 static real_T c_xq2;
6563 static real_T c_yq2;
6564 static real_T c_fif;
6565 static real_T c_fi2;
6566 static real_T c_Lamda2;
6567 static real_T c_L;
6568 static real_T c_x0_dot;
6569 static real_T c_y0_dot;
6570 static real_T c_tx_dot;
6571 static real_T c_ty_dot;
6572
6573 /* Rmax = sqrt((xs-xe)*(xs-xe) +(ys-ye)*(ys-ye)); */
6574 /* if Rmin > Rmax */
6575 /* Rmin = Rmax; */
6576 /* end */
6577 AFCS_MODEL1_PGT_Dubins_2Dmin(xs, ys, vxs, vys, xe, ye, vxe, vye, Rmin, &b_xq1,
6578 &b_yq1, &b_fis, &b_fi1, &b_Lamda1, &b_xq2, &b_yq2, &b_fif, &b_fi2, &b_Lamda2,
6579 &b_L, &b_x0_dot, &b_y0_dot, &b_tx_dot, &b_ty_dot);
6580 *xq1 = b_xq1;
6581 *yq1 = b_yq1;
6582 *fis = b_fis;
6583 fi1 = b_fi1;
6584 *Lamda1 = b_Lamda1;
6585 *xq2 = b_xq2;
6586 *yq2 = b_yq2;
6587 fif = b_fif;
6588 *fi2 = b_fi2;
6589 *Lamda2 = b_Lamda2;
6590 L = b_L;
6591 *x0_dot = b_x0_dot;
6592 *y0_dot = b_y0_dot;
6593 *tx_dot = b_tx_dot;
6594 *ty_dot = b_ty_dot;
6595
6596 /* 2018/10/09修改 */
6597 count = 0;
6598 while ((L > 9.999999E+6) && (count < 10)) {
6599 /* 如果计算的L超过最大值,重新计算 */
6600 Rmin *= 0.9;
6601 AFCS_MODEL1_PGT_Dubins_2Dmin(xs, ys, vxs, vys, xe, ye, vxe, vye, Rmin,
6602 &c_xq1, &c_yq1, &c_fis, &c_fi1, &c_Lamda1, &c_xq2, &c_yq2, &c_fif, &c_fi2,
6603 &c_Lamda2, &c_L, &c_x0_dot, &c_y0_dot, &c_tx_dot, &c_ty_dot);
6604 *xq1 = c_xq1;
6605 *yq1 = c_yq1;
6606 *fis = c_fis;
6607 fi1 = c_fi1;
6608 *Lamda1 = c_Lamda1;
6609 *xq2 = c_xq2;
6610 *yq2 = c_yq2;
6611 fif = c_fif;
6612 *fi2 = c_fi2;
6613 *Lamda2 = c_Lamda2;
6614 L = c_L;
6615 *x0_dot = c_x0_dot;
6616 *y0_dot = c_y0_dot;
6617 *tx_dot = c_tx_dot;
6618 *ty_dot = c_ty_dot;
6619 count++;
6620 }
6621
6622 *R = Rmin;
6623 if (fabs(he - hs) <= L * 1.5574077246549023) {
6624 *Gama = atan((he - hs) / L);
6625 } else {
6626 /* %%二分法求R和Gama */
6627 if (he >= hs) {
6628 *Gama = 1.0;
6629 } else {
6630 *Gama = -1.0;
6631 }
6632 }
6633
6634 /* %计算直线末端高度 */
6635 /* %start */
6636 if (*Lamda1 == 1.0) {
6637 *fi1sum = ((6.2831853071795862 + *fis) - fi1) / 6.2831853071795862;
6638 if (fabs(*fi1sum - AFCS_MODEL1_rt_roundd_snf(*fi1sum)) <=
6639 2.2204460492503131E-16 * fabs(*fi1sum)) {
6640 *fi1sum = 0.0;
6641 } else {
6642 *fi1sum = (*fi1sum - floor(*fi1sum)) * 6.2831853071795862;
6643 }
6644 } else {
6645 *fi1sum = ((6.2831853071795862 + fi1) - *fis) / 6.2831853071795862;
6646 if (fabs(*fi1sum - AFCS_MODEL1_rt_roundd_snf(*fi1sum)) <=
6647 2.2204460492503131E-16 * fabs(*fi1sum)) {
6648 *fi1sum = 0.0;
6649 } else {
6650 *fi1sum = (*fi1sum - floor(*fi1sum)) * 6.2831853071795862;
6651 }
6652 }
6653
6654 if (fabs(fi1 - *fis) < 1.0E-7) {
6655 *fi1sum = 0.0;
6656 }
6657
6658 *hq1 = hs;
6659
6660 /* %end */
6661 if (*Lamda2 == 1.0) {
6662 *fi2sum = ((6.2831853071795862 + *fi2) - fif) / 6.2831853071795862;
6663 if (fabs(*fi2sum - AFCS_MODEL1_rt_roundd_snf(*fi2sum)) <=
6664 2.2204460492503131E-16 * fabs(*fi2sum)) {
6665 *fi2sum = 0.0;
6666 } else {
6667 *fi2sum = (*fi2sum - floor(*fi2sum)) * 6.2831853071795862;
6668 }
6669 } else {
6670 *fi2sum = ((6.2831853071795862 + fif) - *fi2) / 6.2831853071795862;
6671 if (fabs(*fi2sum - AFCS_MODEL1_rt_roundd_snf(*fi2sum)) <=
6672 2.2204460492503131E-16 * fabs(*fi2sum)) {
6673 *fi2sum = 0.0;
6674 } else {
6675 *fi2sum = (*fi2sum - floor(*fi2sum)) * 6.2831853071795862;
6676 }
6677 }
6678
6679 if (fabs(fif - *fi2) < 1.0E-7) {
6680 *fi2sum = 0.0;
6681 }
6682
6683 *hq2 = he - *fi2sum * Rmin * tan(*Gama);
6684
6685 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
6686}
6687
6688/* System initialize for root system: '<Root>' */
6689#define MDL_INITIALIZE_CONDITIONS
6690
6691static void mdlInitializeConditions(SimStruct *S)
6692{
6693 if (ssIsFirstInitCond(S)) {
6694 B_AFCS_MODEL1_T *_rtB;
6695 X_AFCS_MODEL1_T *_rtX;
6696 _rtX = ((X_AFCS_MODEL1_T *) ssGetContStates(S));
6697 _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));
6698
6699 /* InitializeConditions for Memory: '<S433>/Memory10' */
6700 ((real32_T *)ssGetDWork(S, 270))[0] = 0.0F;
6701
6702 /* InitializeConditions for Memory: '<S438>/Memory1' */
6703 ((real32_T *)ssGetDWork(S, 271))[0] = 0.0F;
6704
6705 /* InitializeConditions for Memory: '<S437>/Memory1' */
6706 ((real32_T *)ssGetDWork(S, 272))[0] = 0.0F;
6707
6708 /* InitializeConditions for Memory: '<S436>/Memory1' */
6709 ((real32_T *)ssGetDWork(S, 273))[0] = 0.0F;
6710
6711 /* InitializeConditions for Memory: '<S435>/Memory1' */
6712 ((real32_T *)ssGetDWork(S, 274))[0] = 0.0F;
6713
6714 /* InitializeConditions for Memory: '<S434>/Memory1' */
6715 ((real32_T *)ssGetDWork(S, 275))[0] = 0.0F;
6716
6717 /* InitializeConditions for Memory: '<S439>/Memory1' */
6718 ((real32_T *)ssGetDWork(S, 276))[0] = 0.0F;
6719
6720 /* InitializeConditions for UnitDelay: '<S24>/Unit Delay3' */
6721 ((int8_T *)ssGetDWork(S, 352))[0] = 0;
6722
6723 /* InitializeConditions for UnitDelay: '<S7>/Unit Delay' */
6724 ((int8_T *)ssGetDWork(S, 353))[0] = 1;
6725
6726 /* InitializeConditions for UnitDelay: '<S7>/Unit Delay3' */
6727 ((int8_T *)ssGetDWork(S, 354))[0] = 0;
6728
6729 /* InitializeConditions for Memory: '<S9>/Memory1' */
6730 ((boolean_T *)ssGetDWork(S, 612))[0] = false;
6731
6732 /* InitializeConditions for Memory: '<S9>/Memory2' */
6733 ((int8_T *)ssGetDWork(S, 504))[0] = 0;
6734
6735 /* InitializeConditions for UnitDelay: '<S9>/Unit Delay' */
6736 ((int8_T *)ssGetDWork(S, 355))[0] = 0;
6737
6738 /* InitializeConditions for UnitDelay: '<S1>/Unit Delay1' */
6739 ((int8_T *)ssGetDWork(S, 356))[0] = 0;
6740
6741 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay' */
6742 ((int8_T *)ssGetDWork(S, 357))[0] = 0;
6743
6744 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay1' */
6745 ((int8_T *)ssGetDWork(S, 358))[0] = 0;
6746
6747 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay2' */
6748 ((int8_T *)ssGetDWork(S, 359))[0] = 0;
6749
6750 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay3' */
6751 ((int8_T *)ssGetDWork(S, 360))[0] = 0;
6752
6753 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay4' */
6754 ((int8_T *)ssGetDWork(S, 361))[0] = 0;
6755
6756 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay7' */
6757 ((int8_T *)ssGetDWork(S, 362))[0] = 0;
6758
6759 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay5' */
6760 ((int8_T *)ssGetDWork(S, 363))[0] = 0;
6761
6762 /* InitializeConditions for UnitDelay: '<S32>/Unit Delay3' */
6763 ((int8_T *)ssGetDWork(S, 364))[0] = 0;
6764
6765 /* InitializeConditions for Delay: '<S310>/Delay' */
6766 ((boolean_T *)ssGetDWork(S, 502))[0] = false;
6767
6768 /* InitializeConditions for UnitDelay: '<S309>/Unit Delay6' */
6769 ((int8_T *)ssGetDWork(S, 365))[0] = 0;
6770
6771 /* InitializeConditions for UnitDelay: '<S309>/Unit Delay5' */
6772 ((int8_T *)ssGetDWork(S, 366))[0] = 0;
6773
6774 /* InitializeConditions for UnitDelay: '<S1>/Unit Delay' */
6775 ((int8_T *)ssGetDWork(S, 367))[0] = 0;
6776
6777 /* InitializeConditions for Delay: '<S431>/Delay' */
6778 ((boolean_T *)ssGetDWork(S, 503))[0] = false;
6779
6780 /* InitializeConditions for UnitDelay: '<S430>/Unit Delay6' */
6781 ((int8_T *)ssGetDWork(S, 368))[0] = 0;
6782
6783 /* InitializeConditions for UnitDelay: '<S336>/Unit Delay2' */
6784 ((int8_T *)ssGetDWork(S, 369))[0] = 0;
6785
6786 /* InitializeConditions for UnitDelay: '<S430>/Unit Delay5' */
6787 ((int8_T *)ssGetDWork(S, 370))[0] = 0;
6788
6789 /* InitializeConditions for Memory: '<S433>/Memory1' */
6790 ((real32_T *)ssGetDWork(S, 277))[0] = 0.0F;
6791
6792 /* InitializeConditions for Memory: '<S433>/Memory2' */
6793 ((real32_T *)ssGetDWork(S, 278))[0] = 0.0F;
6794
6795 /* InitializeConditions for Memory: '<S433>/Memory3' */
6796 ((real32_T *)ssGetDWork(S, 279))[0] = 0.0F;
6797
6798 /* InitializeConditions for Memory: '<S433>/Memory4' */
6799 ((real32_T *)ssGetDWork(S, 280))[0] = 0.0F;
6800
6801 /* InitializeConditions for Memory: '<S433>/Memory5' */
6802 ((real32_T *)ssGetDWork(S, 281))[0] = 0.0F;
6803
6804 /* InitializeConditions for Memory: '<S433>/Memory6' */
6805 ((real32_T *)ssGetDWork(S, 282))[0] = 0.0F;
6806
6807 /* InitializeConditions for Memory: '<S433>/Memory7' */
6808 ((real32_T *)ssGetDWork(S, 283))[0] = 0.0F;
6809
6810 /* InitializeConditions for Memory: '<S433>/Memory8' */
6811 ((real32_T *)ssGetDWork(S, 284))[0] = 0.0F;
6812
6813 /* InitializeConditions for Memory: '<S433>/Memory9' */
6814 ((real32_T *)ssGetDWork(S, 285))[0] = 0.0F;
6815
6816 /* SystemInitialize for Atomic SubSystem: '<S1>/Subsystem5' */
6817 /* SystemInitialize for Enabled SubSystem: '<S15>/Subsystem3' */
6818 /* InitializeConditions for Integrator: '<S472>/Servo rate' */
6819 _rtX->Servorate_CSTATE = 0.0;
6820
6821 /* InitializeConditions for Integrator: '<S472>/Limited Deflection' */
6822 _rtX->LimitedDeflection_CSTATE = 0.0;
6823
6824 /* InitializeConditions for Integrator: '<S473>/Servo rate' */
6825 _rtX->Servorate_CSTATE_n = 0.0;
6826
6827 /* InitializeConditions for Integrator: '<S473>/Limited Deflection' */
6828 _rtX->LimitedDeflection_CSTATE_i = 0.0;
6829
6830 /* InitializeConditions for Integrator: '<S474>/Servo rate' */
6831 _rtX->Servorate_CSTATE_m = 0.0;
6832
6833 /* InitializeConditions for Integrator: '<S474>/Limited Deflection' */
6834 _rtX->LimitedDeflection_CSTATE_m = 0.0;
6835
6836 /* InitializeConditions for Integrator: '<S475>/Servo rate' */
6837 _rtX->Servorate_CSTATE_mi = 0.0;
6838
6839 /* InitializeConditions for Integrator: '<S475>/Limited Deflection' */
6840 _rtX->LimitedDeflection_CSTATE_f = 0.0;
6841
6842 /* InitializeConditions for Integrator: '<S476>/Servo rate' */
6843 _rtX->Servorate_CSTATE_f = 0.0;
6844
6845 /* InitializeConditions for Integrator: '<S476>/Limited Deflection' */
6846 _rtX->LimitedDeflection_CSTATE_g = 0.0;
6847
6848 /* InitializeConditions for Integrator: '<S477>/Servo rate' */
6849 _rtX->Servorate_CSTATE_k = 0.0;
6850
6851 /* InitializeConditions for Integrator: '<S477>/Limited Deflection' */
6852 _rtX->LimitedDeflection_CSTATE_l = 0.0;
6853
6854 /* InitializeConditions for Integrator: '<S478>/Servo rate' */
6855 _rtX->Servorate_CSTATE_h = 0.0;
6856
6857 /* InitializeConditions for Integrator: '<S478>/Limited Deflection' */
6858 _rtX->LimitedDeflection_CSTATE_j = 0.0;
6859
6860 /* InitializeConditions for Integrator: '<S479>/Servo rate' */
6861 _rtX->Servorate_CSTATE_kx = 0.0;
6862
6863 /* InitializeConditions for Integrator: '<S479>/Limited Deflection' */
6864 _rtX->LimitedDeflection_CSTATE_la = 0.0;
6865
6866 /* InitializeConditions for TransferFcn: '<S462>/TransferFcn1' */
6867 _rtX->TransferFcn1_CSTATE = 0.0;
6868
6869 /* InitializeConditions for Integrator: '<S779>/q' incorporates:
6870 * InitializeConditions for Integrator: '<S778>/p1'
6871 */
6872 if (ssIsFirstInitCond(S)) {
6873 _rtX->q_CSTATE[0] = 0.0;
6874 _rtX->q_CSTATE[1] = 0.0;
6875 _rtX->q_CSTATE[2] = 0.0;
6876 _rtX->q_CSTATE[3] = 0.0;
6877 _rtX->p1_CSTATE[0] = 0.0;
6878 _rtX->p1_CSTATE[1] = 0.0;
6879 _rtX->p1_CSTATE[2] = 0.0;
6880 }
6881
6882 ((int_T *)ssGetDWork(S, 344))[0] = 1;
6883
6884 /* End of InitializeConditions for Integrator: '<S779>/q' */
6885
6886 /* InitializeConditions for Integrator: '<S778>/p1' */
6887 ((int_T *)ssGetDWork(S, 345))[0] = 1;
6888
6889 /* InitializeConditions for RateLimiter: '<S465>/Rate Limiter' */
6890 ((real_T *)ssGetDWork(S, 3))[0] = (rtInf);
6891
6892 /* InitializeConditions for Integrator: '<S780>/ub,vb,wb' incorporates:
6893 * InitializeConditions for Integrator: '<S470>/p,q,r '
6894 */
6895 if (ssIsFirstInitCond(S)) {
6896 _rtX->ubvbwb_CSTATE[0] = 0.0;
6897 _rtX->ubvbwb_CSTATE[1] = 0.0;
6898 _rtX->ubvbwb_CSTATE[2] = 0.0;
6899 _rtX->pqr_CSTATE[0] = 0.0;
6900 _rtX->pqr_CSTATE[1] = 0.0;
6901 _rtX->pqr_CSTATE[2] = 0.0;
6902 }
6903
6904 ((int_T *)ssGetDWork(S, 346))[0] = 1;
6905
6906 /* End of InitializeConditions for Integrator: '<S780>/ub,vb,wb' */
6907
6908 /* InitializeConditions for Backlash: '<S446>/Backlash' */
6909 ((real_T *)ssGetDWork(S, 4))[0] = 0.0;
6910
6911 /* InitializeConditions for Integrator: '<S470>/p,q,r ' */
6912 ((int_T *)ssGetDWork(S, 347))[0] = 1;
6913
6914 /* InitializeConditions for UnitDelay: '<S682>/Unit Delay' */
6915 ((int8_T *)ssGetDWork(S, 371))[0] = 0;
6916
6917 /* InitializeConditions for UnitDelay: '<S683>/Unit Delay' */
6918 ((int8_T *)ssGetDWork(S, 372))[0] = 0;
6919
6920 /* InitializeConditions for UnitDelay: '<S619>/Unit Delay1' */
6921 ((real_T *)ssGetDWork(S, 0))[0] = 0.0;
6922
6923 /* InitializeConditions for Integrator: '<S683>/Integrator' incorporates:
6924 * InitializeConditions for Integrator: '<S682>/Integrator'
6925 */
6926 if (ssIsFirstInitCond(S)) {
6927 _rtX->Integrator_CSTATE = 0.0;
6928 _rtX->Integrator_CSTATE_l = 0.0;
6929 }
6930
6931 ((int_T *)ssGetDWork(S, 348))[0] = 1;
6932
6933 /* End of InitializeConditions for Integrator: '<S683>/Integrator' */
6934
6935 /* InitializeConditions for Integrator: '<S682>/Integrator' */
6936 ((int_T *)ssGetDWork(S, 349))[0] = 1;
6937
6938 /* InitializeConditions for TransferFcn: '<S683>/Transfer Fcn2' */
6939 _rtX->TransferFcn2_CSTATE[0] = 0.0;
6940 _rtX->TransferFcn2_CSTATE[1] = 0.0;
6941
6942 /* InitializeConditions for Integrator: '<S781>/Integrator' */
6943 _rtX->Integrator_CSTATE_a = 0.0;
6944
6945 /* InitializeConditions for Integrator: '<S778>/p' */
6946 if (ssIsFirstInitCond(S)) {
6947 _rtX->p_CSTATE[0] = 0.0;
6948 _rtX->p_CSTATE[1] = 0.0;
6949 _rtX->p_CSTATE[2] = 0.0;
6950 }
6951
6952 ((int_T *)ssGetDWork(S, 350))[0] = 1;
6953
6954 /* End of InitializeConditions for Integrator: '<S778>/p' */
6955
6956 /* InitializeConditions for TransferFcn: '<S446>/Transfer Fcn' */
6957 _rtX->TransferFcn_CSTATE = 0.0;
6958
6959 /* SystemInitialize for Merge: '<S847>/Merge' */
6960 _rtB->Merge[0] = 1.0;
6961 _rtB->Merge[1] = 0.0;
6962 _rtB->Merge[2] = 0.0;
6963 _rtB->Merge[3] = 0.0;
6964
6965 /* SystemInitialize for Iterator SubSystem: '<S774>/While Iterator Subsystem' */
6966 /* InitializeConditions for Memory: '<S793>/Memory' */
6967 ((real_T *)ssGetDWork(S, 11))[0] = 0.0;
6968
6969 /* SystemInitialize for Outport: '<S787>/phi' */
6970 _rtB->TrigonometricFunction4 = 0.0;
6971
6972 /* End of SystemInitialize for SubSystem: '<S774>/While Iterator Subsystem' */
6973
6974 /* SystemInitialize for Atomic SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */
6975 AFCS_MODEL1_LMainGearGeometryForcesandMoments_Init(S,
6976 &_rtB->LMainGearGeometryForcesandMomen);
6977
6978 /* End of SystemInitialize for SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */
6979
6980 /* SystemInitialize for Atomic SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */
6981 AFCS_MODEL1_RMainGearGeometryForcesandMoments_Init(S,
6982 &_rtB->RMainGearGeometryForcesandMomen);
6983
6984 /* End of SystemInitialize for SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */
6985
6986 /* SystemInitialize for Atomic SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */
6987 AFCS_MODEL1_NoseGearGeometryForcesandMoments_Init(S,
6988 &_rtB->NoseGearGeometryForcesandMoment);
6989
6990 /* End of SystemInitialize for SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */
6991
6992 /* SystemInitialize for Outport: '<S446>/Sensor1' */
6993 _rtB->phiL = 0.0;
6994 _rtB->thetaL = 0.0;
6995 _rtB->psiL = 0.0;
6996 _rtB->p = 0.0;
6997 _rtB->q = 0.0;
6998 _rtB->r = 0.0;
6999 _rtB->Abx = 0.0;
7000 _rtB->Aby = 0.0;
7001 _rtB->Abz = 0.0;
7002 _rtB->Lat_j = 0.0;
7003 _rtB->Lon_e = 0.0;
7004 _rtB->Altitude_j = 0.0;
7005 _rtB->GPS_vnorth_i = 0.0;
7006 _rtB->GPS_veast_k = 0.0;
7007 _rtB->GPS_vdown_k = 0.0;
7008 _rtB->Alfa = 0.0;
7009 _rtB->Beta = 0.0;
7010 _rtB->Qbar = 0.0;
7011 _rtB->Va = 0.0;
7012
7013 /* SystemInitialize for Outport: '<S446>/ENT' */
7014 _rtB->OutportBufferForENT[0] = 0.0;
7015 _rtB->OutportBufferForENT[1] = 0.0;
7016 _rtB->OutportBufferForENT[2] = 0.0;
7017
7018 /* End of SystemInitialize for SubSystem: '<S15>/Subsystem3' */
7019 /* End of SystemInitialize for SubSystem: '<S1>/Subsystem5' */
7020
7021 /* SystemInitialize for Enabled SubSystem: '<S299>/Enabled Subsystem' */
7022 /* SystemInitialize for Outport: '<S302>/A' */
7023 memset(&_rtB->OutportBufferForA[0], 0, 120U * sizeof(real_T));
7024
7025 /* SystemInitialize for Outport: '<S302>/NumPoint' */
7026 _rtB->airNumPoint = 0;
7027
7028 /* End of SystemInitialize for SubSystem: '<S299>/Enabled Subsystem' */
7029
7030 /* SystemInitialize for MATLAB Function: '<S293>/CompTP' */
7031 ((int8_T *)ssGetDWork(S, 516))[0] = 0;
7032 ((real32_T *)ssGetDWork(S, 287))[0] = 0.0F;
7033 ((real32_T *)ssGetDWork(S, 288))[0] = 0.0F;
7034
7035 /* SystemInitialize for Chart: '<S24>/zeros2' */
7036 ((uint8_T *)ssGetDWork(S, 586))[0] = 0U;
7037 ((uint8_T *)ssGetDWork(S, 587))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7038
7039 /* SystemInitialize for Atomic SubSystem: '<S9>/Trajectory planning,tracking' */
7040 /* InitializeConditions for UnitDelay: '<S423>/Output' */
7041 ((uint64_T *)ssGetDWork(S, 1))[0] = 0ULL;
7042
7043 /* End of SystemInitialize for SubSystem: '<S9>/Trajectory planning,tracking' */
7044
7045 /* SystemInitialize for Chart: '<S21>/delayblock3' */
7046 ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7047 ((uint8_T *)ssGetDWork(S, 603))[0] = 0U;
7048 ((uint8_T *)ssGetDWork(S, 604))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7049
7050 /* SystemInitialize for MATLAB Function: '<S31>/LogicTrans' */
7051 ((int8_T *)ssGetDWork(S, 514))[0] = 0;
7052
7053 /* SystemInitialize for Chart: '<S21>/delayblock2' */
7054 ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7055 ((uint8_T *)ssGetDWork(S, 606))[0] = 0U;
7056 ((uint8_T *)ssGetDWork(S, 607))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7057
7058 /* SystemInitialize for Chart: '<S32>/wowcmp1' */
7059 ((uint8_T *)ssGetDWork(S, 526))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7060 ((uint8_T *)ssGetDWork(S, 524))[0] = 0U;
7061 ((uint8_T *)ssGetDWork(S, 525))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7062
7063 /* SystemInitialize for Chart: '<S21>/delayblock5' */
7064 ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7065 ((uint8_T *)ssGetDWork(S, 597))[0] = 0U;
7066 ((uint8_T *)ssGetDWork(S, 598))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7067
7068 /* SystemInitialize for Chart: '<S21>/delayblock7' */
7069 ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7070 ((uint8_T *)ssGetDWork(S, 591))[0] = 0U;
7071 ((uint8_T *)ssGetDWork(S, 592))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7072
7073 /* SystemInitialize for Chart: '<S21>/delayblock4' */
7074 ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7075 ((uint8_T *)ssGetDWork(S, 600))[0] = 0U;
7076 ((uint8_T *)ssGetDWork(S, 601))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7077
7078 /* SystemInitialize for IfAction SubSystem: '<S31>/Pre-Takeoff' */
7079 /* SystemInitialize for Chart: '<S319>/Pre_Takeoff' */
7080 ((uint8_T *)ssGetDWork(S, 582))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7081 ((uint8_T *)ssGetDWork(S, 580))[0] = 0U;
7082 ((uint8_T *)ssGetDWork(S, 581))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7083
7084 /* End of SystemInitialize for SubSystem: '<S31>/Pre-Takeoff' */
7085
7086 /* SystemInitialize for IfAction SubSystem: '<S31>/Takeoff' */
7087 /* SystemInitialize for Chart: '<S321>/Takeoff' */
7088 ((uint8_T *)ssGetDWork(S, 578))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7089 ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7090 ((uint8_T *)ssGetDWork(S, 576))[0] = 0U;
7091 ((uint8_T *)ssGetDWork(S, 577))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7092
7093 /* End of SystemInitialize for SubSystem: '<S31>/Takeoff' */
7094
7095 /* SystemInitialize for IfAction SubSystem: '<S31>/Climb' */
7096 /* SystemInitialize for Chart: '<S314>/Climb2' */
7097 ((uint8_T *)ssGetDWork(S, 569))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7098 ((uint8_T *)ssGetDWork(S, 570))[0] = 0U;
7099 ((uint8_T *)ssGetDWork(S, 575))[0] = 0U;
7100 ((uint8_T *)ssGetDWork(S, 572))[0] = 0U;
7101 ((uint8_T *)ssGetDWork(S, 571))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7102 ((uint8_T *)ssGetDWork(S, 573))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7103 ((uint8_T *)ssGetDWork(S, 574))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7104 ((uint8_T *)ssGetDWork(S, 567))[0] = 0U;
7105 ((uint8_T *)ssGetDWork(S, 568))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7106
7107 /* End of SystemInitialize for SubSystem: '<S31>/Climb' */
7108
7109 /* SystemInitialize for IfAction SubSystem: '<S31>/Cruise' */
7110 /* SystemInitialize for Chart: '<S315>/Cruise' */
7111 ((uint8_T *)ssGetDWork(S, 560))[0] = 0U;
7112 ((uint8_T *)ssGetDWork(S, 559))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7113 ((uint8_T *)ssGetDWork(S, 561))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7114 ((uint8_T *)ssGetDWork(S, 563))[0] = 0U;
7115 ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7116 ((uint8_T *)ssGetDWork(S, 565))[0] = 0U;
7117 ((uint8_T *)ssGetDWork(S, 564))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7118 ((uint8_T *)ssGetDWork(S, 566))[0] = 0U;
7119 ((uint8_T *)ssGetDWork(S, 557))[0] = 0U;
7120 ((uint8_T *)ssGetDWork(S, 558))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7121
7122 /* End of SystemInitialize for SubSystem: '<S31>/Cruise' */
7123
7124 /* SystemInitialize for IfAction SubSystem: '<S31>/Approach' */
7125 /* SystemInitialize for Chart: '<S313>/Approach2' */
7126 ((uint8_T *)ssGetDWork(S, 549))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7127 ((uint8_T *)ssGetDWork(S, 550))[0] = 0U;
7128 ((uint8_T *)ssGetDWork(S, 552))[0] = 0U;
7129 ((uint8_T *)ssGetDWork(S, 551))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7130 ((uint8_T *)ssGetDWork(S, 555))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7131 ((uint8_T *)ssGetDWork(S, 556))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7132 ((uint8_T *)ssGetDWork(S, 553))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7133 ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7134 ((uint8_T *)ssGetDWork(S, 547))[0] = 0U;
7135 ((uint8_T *)ssGetDWork(S, 548))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7136
7137 /* End of SystemInitialize for SubSystem: '<S31>/Approach' */
7138
7139 /* SystemInitialize for IfAction SubSystem: '<S31>/Landing' */
7140 /* SystemInitialize for Chart: '<S316>/Landing2' */
7141 ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7142 ((uint8_T *)ssGetDWork(S, 539))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7143 ((uint8_T *)ssGetDWork(S, 540))[0] = 0U;
7144 ((uint8_T *)ssGetDWork(S, 542))[0] = 0U;
7145 ((uint8_T *)ssGetDWork(S, 541))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7146 ((uint8_T *)ssGetDWork(S, 545))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7147 ((uint8_T *)ssGetDWork(S, 546))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7148 ((uint8_T *)ssGetDWork(S, 543))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7149 ((uint8_T *)ssGetDWork(S, 544))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7150 ((uint8_T *)ssGetDWork(S, 535))[0] = 0U;
7151 ((uint8_T *)ssGetDWork(S, 537))[0] = 0U;
7152 ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7153 ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7154 ((uint8_T *)ssGetDWork(S, 532))[0] = 0U;
7155 ((uint8_T *)ssGetDWork(S, 533))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7156
7157 /* End of SystemInitialize for SubSystem: '<S31>/Landing' */
7158
7159 /* SystemInitialize for IfAction SubSystem: '<S31>/Taxi' */
7160 /* SystemInitialize for Chart: '<S322>/Taxi' */
7161 ((uint8_T *)ssGetDWork(S, 531))[0] = 0U;
7162 ((uint8_T *)ssGetDWork(S, 530))[0] = 0U;
7163 ((uint8_T *)ssGetDWork(S, 529))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7164 ((uint8_T *)ssGetDWork(S, 527))[0] = 0U;
7165 ((uint8_T *)ssGetDWork(S, 528))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7166
7167 /* End of SystemInitialize for SubSystem: '<S31>/Taxi' */
7168
7169 /* SystemInitialize for Merge: '<S31>/Merge' */
7170 _rtB->vtcmd = 0.0F;
7171
7172 /* SystemInitialize for Merge: '<S31>/Merge' */
7173 _rtB->nycmd = 0.0F;
7174
7175 /* SystemInitialize for Merge: '<S31>/Merge' */
7176 _rtB->phiLcmd = 0.0F;
7177
7178 /* SystemInitialize for Merge: '<S31>/Merge' */
7179 _rtB->thetaLcmd = 0.0F;
7180
7181 /* SystemInitialize for Merge: '<S31>/Merge' */
7182 _rtB->psiLcmd = 0.0F;
7183
7184 /* SystemInitialize for Merge: '<S31>/Merge' */
7185 _rtB->gamacmd = 0.0F;
7186
7187 /* SystemInitialize for Merge: '<S31>/Merge' */
7188 _rtB->hcmd = 0.0F;
7189
7190 /* SystemInitialize for Merge: '<S31>/Merge' */
7191 _rtB->psdcmd = 0.0F;
7192
7193 /* SystemInitialize for Merge: '<S31>/Merge' */
7194 _rtB->yrwcmd = 0.0F;
7195
7196 /* SystemInitialize for Merge: '<S31>/Merge' */
7197 _rtB->apthL = 0;
7198
7199 /* SystemInitialize for Merge: '<S31>/Merge' */
7200 _rtB->apfix = 0;
7201
7202 /* SystemInitialize for Merge: '<S31>/Merge' */
7203 _rtB->aplonL = 0;
7204
7205 /* SystemInitialize for Merge: '<S31>/Merge' */
7206 _rtB->aplatL = 0;
7207
7208 /* SystemInitialize for Merge: '<S31>/Merge' */
7209 _rtB->apyawL = 0;
7210
7211 /* SystemInitialize for Merge: '<S31>/Merge' */
7212 _rtB->apsbL = 0;
7213
7214 /* SystemInitialize for Merge: '<S31>/Merge' */
7215 _rtB->apnwsL = 0;
7216
7217 /* SystemInitialize for Merge: '<S31>/Merge' */
7218 _rtB->apbrkL = 0;
7219
7220 /* SystemInitialize for Merge: '<S31>/Merge' */
7221 _rtB->thrustcmd = 0.0F;
7222
7223 /* SystemInitialize for Atomic SubSystem: '<S7>/Level' */
7224
7225 /* SystemInitialize for Atomic SubSystem: '<S22>/CodeReuseSubsystem' */
7226 AFCS_MODEL1_CodeReuseSubsystem_Init(S);
7227
7228 /* End of SystemInitialize for SubSystem: '<S22>/CodeReuseSubsystem' */
7229
7230 /* End of SystemInitialize for SubSystem: '<S7>/Level' */
7231
7232 /* SystemInitialize for Merge: '<S31>/Merge' */
7233 _rtB->configure = 0;
7234
7235 /* SystemInitialize for Merge: '<S31>/Merge' */
7236 _rtB->nzcmd = 0.0F;
7237
7238 /* SystemInitialize for Merge: '<S31>/Merge' */
7239 _rtB->K = 0;
7240
7241 /* SystemInitialize for Chart: '<S21>/delayblock1' */
7242 ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7243 ((uint8_T *)ssGetDWork(S, 609))[0] = 0U;
7244 ((uint8_T *)ssGetDWork(S, 610))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7245
7246 /* SystemInitialize for Chart: '<S21>/delayblock6' */
7247 ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7248 ((uint8_T *)ssGetDWork(S, 594))[0] = 0U;
7249 ((uint8_T *)ssGetDWork(S, 595))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7250
7251 /* SystemInitialize for Chart: '<S299>/AirLineUpdateSucModel' */
7252 ((uint8_T *)ssGetDWork(S, 590))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7253 ((uint8_T *)ssGetDWork(S, 588))[0] = 0U;
7254 ((uint8_T *)ssGetDWork(S, 589))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7255
7256 /* SystemInitialize for Chart: '<S29>/LLACalibration2' */
7257 ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7258 ((uint8_T *)ssGetDWork(S, 583))[0] = 0U;
7259 ((uint8_T *)ssGetDWork(S, 584))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7260
7261 /* SystemInitialize for Merge: '<S31>/Merge' */
7262 _rtB->apcheck = 0;
7263
7264 /* SystemInitialize for Merge: '<S31>/Merge' */
7265 _rtB->StartEmNav = 0;
7266
7267 /* SystemInitialize for Chart: '<S12>/LLACalibration2' */
7268 ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7269 ((uint8_T *)ssGetDWork(S, 521))[0] = 0U;
7270 ((uint8_T *)ssGetDWork(S, 522))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7271 } else {
7272 /* InitializeConditions for Memory: '<S433>/Memory10' */
7273 ((real32_T *)ssGetDWork(S, 270))[0] = 0.0F;
7274
7275 /* InitializeConditions for Memory: '<S438>/Memory1' */
7276 ((real32_T *)ssGetDWork(S, 271))[0] = 0.0F;
7277
7278 /* InitializeConditions for Memory: '<S437>/Memory1' */
7279 ((real32_T *)ssGetDWork(S, 272))[0] = 0.0F;
7280
7281 /* InitializeConditions for Memory: '<S436>/Memory1' */
7282 ((real32_T *)ssGetDWork(S, 273))[0] = 0.0F;
7283
7284 /* InitializeConditions for Memory: '<S435>/Memory1' */
7285 ((real32_T *)ssGetDWork(S, 274))[0] = 0.0F;
7286
7287 /* InitializeConditions for Memory: '<S434>/Memory1' */
7288 ((real32_T *)ssGetDWork(S, 275))[0] = 0.0F;
7289
7290 /* InitializeConditions for Memory: '<S439>/Memory1' */
7291 ((real32_T *)ssGetDWork(S, 276))[0] = 0.0F;
7292
7293 /* InitializeConditions for UnitDelay: '<S24>/Unit Delay3' */
7294 ((int8_T *)ssGetDWork(S, 352))[0] = 0;
7295
7296 /* InitializeConditions for UnitDelay: '<S7>/Unit Delay' */
7297 ((int8_T *)ssGetDWork(S, 353))[0] = 1;
7298
7299 /* InitializeConditions for UnitDelay: '<S7>/Unit Delay3' */
7300 ((int8_T *)ssGetDWork(S, 354))[0] = 0;
7301
7302 /* InitializeConditions for Memory: '<S9>/Memory1' */
7303 ((boolean_T *)ssGetDWork(S, 612))[0] = false;
7304
7305 /* InitializeConditions for Memory: '<S9>/Memory2' */
7306 ((int8_T *)ssGetDWork(S, 504))[0] = 0;
7307
7308 /* InitializeConditions for UnitDelay: '<S9>/Unit Delay' */
7309 ((int8_T *)ssGetDWork(S, 355))[0] = 0;
7310
7311 /* InitializeConditions for UnitDelay: '<S1>/Unit Delay1' */
7312 ((int8_T *)ssGetDWork(S, 356))[0] = 0;
7313
7314 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay' */
7315 ((int8_T *)ssGetDWork(S, 357))[0] = 0;
7316
7317 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay1' */
7318 ((int8_T *)ssGetDWork(S, 358))[0] = 0;
7319
7320 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay2' */
7321 ((int8_T *)ssGetDWork(S, 359))[0] = 0;
7322
7323 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay3' */
7324 ((int8_T *)ssGetDWork(S, 360))[0] = 0;
7325
7326 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay4' */
7327 ((int8_T *)ssGetDWork(S, 361))[0] = 0;
7328
7329 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay7' */
7330 ((int8_T *)ssGetDWork(S, 362))[0] = 0;
7331
7332 /* InitializeConditions for UnitDelay: '<S31>/Unit Delay5' */
7333 ((int8_T *)ssGetDWork(S, 363))[0] = 0;
7334
7335 /* InitializeConditions for UnitDelay: '<S32>/Unit Delay3' */
7336 ((int8_T *)ssGetDWork(S, 364))[0] = 0;
7337
7338 /* InitializeConditions for Delay: '<S310>/Delay' */
7339 ((boolean_T *)ssGetDWork(S, 502))[0] = false;
7340
7341 /* InitializeConditions for UnitDelay: '<S309>/Unit Delay6' */
7342 ((int8_T *)ssGetDWork(S, 365))[0] = 0;
7343
7344 /* InitializeConditions for UnitDelay: '<S309>/Unit Delay5' */
7345 ((int8_T *)ssGetDWork(S, 366))[0] = 0;
7346
7347 /* InitializeConditions for UnitDelay: '<S1>/Unit Delay' */
7348 ((int8_T *)ssGetDWork(S, 367))[0] = 0;
7349
7350 /* InitializeConditions for Delay: '<S431>/Delay' */
7351 ((boolean_T *)ssGetDWork(S, 503))[0] = false;
7352
7353 /* InitializeConditions for UnitDelay: '<S430>/Unit Delay6' */
7354 ((int8_T *)ssGetDWork(S, 368))[0] = 0;
7355
7356 /* InitializeConditions for UnitDelay: '<S336>/Unit Delay2' */
7357 ((int8_T *)ssGetDWork(S, 369))[0] = 0;
7358
7359 /* InitializeConditions for UnitDelay: '<S430>/Unit Delay5' */
7360 ((int8_T *)ssGetDWork(S, 370))[0] = 0;
7361
7362 /* InitializeConditions for Memory: '<S433>/Memory1' */
7363 ((real32_T *)ssGetDWork(S, 277))[0] = 0.0F;
7364
7365 /* InitializeConditions for Memory: '<S433>/Memory2' */
7366 ((real32_T *)ssGetDWork(S, 278))[0] = 0.0F;
7367
7368 /* InitializeConditions for Memory: '<S433>/Memory3' */
7369 ((real32_T *)ssGetDWork(S, 279))[0] = 0.0F;
7370
7371 /* InitializeConditions for Memory: '<S433>/Memory4' */
7372 ((real32_T *)ssGetDWork(S, 280))[0] = 0.0F;
7373
7374 /* InitializeConditions for Memory: '<S433>/Memory5' */
7375 ((real32_T *)ssGetDWork(S, 281))[0] = 0.0F;
7376
7377 /* InitializeConditions for Memory: '<S433>/Memory6' */
7378 ((real32_T *)ssGetDWork(S, 282))[0] = 0.0F;
7379
7380 /* InitializeConditions for Memory: '<S433>/Memory7' */
7381 ((real32_T *)ssGetDWork(S, 283))[0] = 0.0F;
7382
7383 /* InitializeConditions for Memory: '<S433>/Memory8' */
7384 ((real32_T *)ssGetDWork(S, 284))[0] = 0.0F;
7385
7386 /* InitializeConditions for Memory: '<S433>/Memory9' */
7387 ((real32_T *)ssGetDWork(S, 285))[0] = 0.0F;
7388
7389 /* SystemReset for MATLAB Function: '<S293>/CompTP' */
7390 ((int8_T *)ssGetDWork(S, 516))[0] = 0;
7391 ((real32_T *)ssGetDWork(S, 287))[0] = 0.0F;
7392 ((real32_T *)ssGetDWork(S, 288))[0] = 0.0F;
7393
7394 /* SystemReset for Chart: '<S24>/zeros2' */
7395 ((uint8_T *)ssGetDWork(S, 586))[0] = 0U;
7396 ((uint8_T *)ssGetDWork(S, 587))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7397
7398 /* SystemReset for Atomic SubSystem: '<S9>/Trajectory planning,tracking' */
7399 /* InitializeConditions for UnitDelay: '<S423>/Output' */
7400 ((uint64_T *)ssGetDWork(S, 1))[0] = 0ULL;
7401
7402 /* End of SystemReset for SubSystem: '<S9>/Trajectory planning,tracking' */
7403
7404 /* SystemReset for Chart: '<S21>/delayblock3' */
7405 ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7406 ((uint8_T *)ssGetDWork(S, 603))[0] = 0U;
7407 ((uint8_T *)ssGetDWork(S, 604))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7408
7409 /* SystemReset for MATLAB Function: '<S31>/LogicTrans' */
7410 ((int8_T *)ssGetDWork(S, 514))[0] = 0;
7411
7412 /* SystemReset for Chart: '<S21>/delayblock2' */
7413 ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7414 ((uint8_T *)ssGetDWork(S, 606))[0] = 0U;
7415 ((uint8_T *)ssGetDWork(S, 607))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7416
7417 /* SystemReset for Chart: '<S32>/wowcmp1' */
7418 ((uint8_T *)ssGetDWork(S, 526))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7419 ((uint8_T *)ssGetDWork(S, 524))[0] = 0U;
7420 ((uint8_T *)ssGetDWork(S, 525))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7421
7422 /* SystemReset for Chart: '<S21>/delayblock5' */
7423 ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7424 ((uint8_T *)ssGetDWork(S, 597))[0] = 0U;
7425 ((uint8_T *)ssGetDWork(S, 598))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7426
7427 /* SystemReset for Chart: '<S21>/delayblock7' */
7428 ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7429 ((uint8_T *)ssGetDWork(S, 591))[0] = 0U;
7430 ((uint8_T *)ssGetDWork(S, 592))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7431
7432 /* SystemReset for Chart: '<S21>/delayblock4' */
7433 ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7434 ((uint8_T *)ssGetDWork(S, 600))[0] = 0U;
7435 ((uint8_T *)ssGetDWork(S, 601))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7436
7437 /* SystemReset for Atomic SubSystem: '<S7>/Level' */
7438
7439 /* SystemReset for Atomic SubSystem: '<S22>/CodeReuseSubsystem' */
7440 AFCS_MODEL1_CodeReuseSubsystem_Reset(S);
7441
7442 /* End of SystemReset for SubSystem: '<S22>/CodeReuseSubsystem' */
7443
7444 /* End of SystemReset for SubSystem: '<S7>/Level' */
7445
7446 /* SystemReset for Chart: '<S21>/delayblock1' */
7447 ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7448 ((uint8_T *)ssGetDWork(S, 609))[0] = 0U;
7449 ((uint8_T *)ssGetDWork(S, 610))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7450
7451 /* SystemReset for Chart: '<S21>/delayblock6' */
7452 ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7453 ((uint8_T *)ssGetDWork(S, 594))[0] = 0U;
7454 ((uint8_T *)ssGetDWork(S, 595))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7455
7456 /* SystemReset for Chart: '<S299>/AirLineUpdateSucModel' */
7457 ((uint8_T *)ssGetDWork(S, 590))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7458 ((uint8_T *)ssGetDWork(S, 588))[0] = 0U;
7459 ((uint8_T *)ssGetDWork(S, 589))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7460
7461 /* SystemReset for Chart: '<S29>/LLACalibration2' */
7462 ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7463 ((uint8_T *)ssGetDWork(S, 583))[0] = 0U;
7464 ((uint8_T *)ssGetDWork(S, 584))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7465
7466 /* SystemReset for Chart: '<S12>/LLACalibration2' */
7467 ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7468 ((uint8_T *)ssGetDWork(S, 521))[0] = 0U;
7469 ((uint8_T *)ssGetDWork(S, 522))[0] = AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
7470 }
7471}
7472
7473/* Disable for root system: '<Root>' */
7474#define RTW_GENERATED_DISABLE
7475
7476static void mdlDisable(SimStruct *S)
7477{
7478 B_AFCS_MODEL1_T *_rtB;
7479 _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));
7480
7481 /* Disable for Atomic SubSystem: '<S1>/Subsystem5' */
7482 /* Disable for Enabled SubSystem: '<S15>/Subsystem3' */
7483 if (((boolean_T *)ssGetDWork(S, 614))[0]) {
7484 /* Disable for If: '<S847>/If' */
7485 ((int8_T *)ssGetDWork(S, 506))[0] = -1;
7486
7487 /* Disable for If: '<S846>/If' */
7488 ((int8_T *)ssGetDWork(S, 507))[0] = -1;
7489
7490 /* Disable for Atomic SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */
7491 AFCS_MODEL1_LMainGearGeometryForcesandMoments_Disable(S,
7492 &_rtB->LMainGearGeometryForcesandMomen);
7493
7494 /* End of Disable for SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */
7495
7496 /* Disable for Atomic SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */
7497 AFCS_MODEL1_RMainGearGeometryForcesandMoments_Disable(S,
7498 &_rtB->RMainGearGeometryForcesandMomen);
7499
7500 /* End of Disable for SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */
7501
7502 /* Disable for Atomic SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */
7503 AFCS_MODEL1_NoseGearGeometryForcesandMoments_Disable(S,
7504 &_rtB->NoseGearGeometryForcesandMoment);
7505
7506 /* End of Disable for SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */
7507
7508 /* Disable for If: '<S848>/If' */
7509 ((int8_T *)ssGetDWork(S, 508))[0] = -1;
7510 ((boolean_T *)ssGetDWork(S, 614))[0] = false;
7511 }
7512
7513 /* End of Disable for SubSystem: '<S15>/Subsystem3' */
7514 /* End of Disable for SubSystem: '<S1>/Subsystem5' */
7515
7516 /* Disable for Enabled SubSystem: '<S299>/Enabled Subsystem' */
7517 if (((boolean_T *)ssGetDWork(S, 613))[0]) {
7518 ((boolean_T *)ssGetDWork(S, 613))[0] = false;
7519 }
7520
7521 /* End of Disable for SubSystem: '<S299>/Enabled Subsystem' */
7522
7523 /* Disable for SwitchCase: '<S31>/Switch Case' */
7524 if (((int8_T *)ssGetDWork(S, 505))[0] == 3) {
7525 ssSetSolverNeedsReset(S);
7526 }
7527
7528 ((int8_T *)ssGetDWork(S, 505))[0] = -1;
7529
7530 /* End of Disable for SwitchCase: '<S31>/Switch Case' */
7531}
7532
7533/* Start for root system: '<Root>' */
7534#define MDL_START
7535
7536static void mdlStart(SimStruct *S)
7537{
7538 /* instance underlying S-Function data */
7539#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)
7540# if defined(MATLAB_MEX_FILE)
7541
7542 /* non-finites */
7543 rt_InitInfAndNaN(sizeof(real_T));
7544
7545 /* Check for invalid switching between solver types */
7546 if (ssIsVariableStepSolver(S)) {
7547 ssSetErrorStatus(S, "This Simulink Coder generated "
7548 "S-Function cannot be used in a simulation with "
7549 "a solver type of variable-step "
7550 "because this S-Function was created from a model with "
7551 "solver type of fixed-step and it has continuous time blocks. "
7552 "See the Solver page of the simulation parameters dialog.");
7553 return;
7554 }
7555
7556 if (ssGetSolverMode(S) == SOLVER_MODE_MULTITASKING) {
7557 ssSetErrorStatus(S, "This Simulink Coder generated "
7558 "S-Function cannot be used in a simulation with "
7559 "solver mode set to auto or multitasking "
7560 "because this S-Function was created from a model with "
7561 "solver mode set to singletasking. "
7562 "See the Solver page of the simulation parameters dialog.");
7563 return;
7564 }
7565
7566# endif
7567
7568 AFCS_MODEL1_malloc(S);
7569 if (ssGetErrorStatus(S) != (NULL) ) {
7570 return;
7571 }
7572
7573#endif
7574
7575 {
7576 B_AFCS_MODEL1_T *_rtB;
7577 _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));
7578
7579 /* Start for Atomic SubSystem: '<S1>/Subsystem5' */
7580 /* Start for Constant: '<S440>/Constant3' */
7581 _rtB->uvw0[0] = 0.0;
7582 _rtB->uvw0[1] = 0.0;
7583 _rtB->uvw0[2] = 0.0;
7584
7585 /* Start for Enabled SubSystem: '<S15>/Subsystem3' */
7586 ((boolean_T *)ssGetDWork(S, 614))[0] = false;
7587 ssSetSolverNeedsReset(S);
7588
7589 /* Start for If: '<S847>/If' */
7590 ((int8_T *)ssGetDWork(S, 506))[0] = -1;
7591
7592 /* Start for IfAction SubSystem: '<S847>/Negative Trace' */
7593 /* Start for If: '<S877>/Find Maximum Diagonal Value' */
7594 ((int8_T *)ssGetDWork(S, 510))[0] = -1;
7595
7596 /* End of Start for SubSystem: '<S847>/Negative Trace' */
7597
7598 /* Start for If: '<S846>/If' */
7599 ((int8_T *)ssGetDWork(S, 507))[0] = -1;
7600
7601 /* Start for Atomic SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */
7602 AFCS_MODEL1_LMainGearGeometryForcesandMoments_Start(S);
7603
7604 /* End of Start for SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */
7605
7606 /* Start for Atomic SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */
7607 AFCS_MODEL1_RMainGearGeometryForcesandMoments_Start(S);
7608
7609 /* End of Start for SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */
7610
7611 /* Start for Atomic SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */
7612 AFCS_MODEL1_NoseGearGeometryForcesandMoments_Start(S);
7613
7614 /* End of Start for SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */
7615
7616 /* Start for If: '<S848>/If' */
7617 ((int8_T *)ssGetDWork(S, 508))[0] = -1;
7618
7619 /* Start for IfAction SubSystem: '<S848>/Negative Trace' */
7620 /* Start for If: '<S902>/Find Maximum Diagonal Value' */
7621 ((int8_T *)ssGetDWork(S, 509))[0] = -1;
7622
7623 /* End of Start for SubSystem: '<S848>/Negative Trace' */
7624 /* End of Start for SubSystem: '<S15>/Subsystem3' */
7625 /* Start for Enabled SubSystem: '<S299>/Enabled Subsystem' */
7626 ((boolean_T *)ssGetDWork(S, 613))[0] = false;
7627 ssSetSolverNeedsReset(S);
7628
7629 /* End of Start for SubSystem: '<S299>/Enabled Subsystem' */
7630 /* Start for DataStoreMemory: '<S335>/Data Store Memory' */
7631 ((uint16_T *)ssGetDWork(S, 351))[0] = 1U;
7632
7633 /* Start for DataStoreMemory: '<S335>/Data Store Memory1' */
7634 ((uint32_T *)ssGetDWork(S, 343))[0] = 0U;
7635
7636 /* Start for DataStoreMemory: '<S335>/Data Store Memory10' */
7637 memset(&((real_T *)ssGetDWork(S, 12))[0], 0, sizeof(real_T) << 5U);
7638
7639 /* Start for DataStoreMemory: '<S335>/Data Store Memory5' */
7640 memset(&((real_T *)ssGetDWork(S, 13))[0], 0, sizeof(real_T) << 5U);
7641
7642 /* Start for DataStoreMemory: '<S335>/Data Store Memory6' */
7643 memset(&((real_T *)ssGetDWork(S, 14))[0], 0, sizeof(real_T) << 5U);
7644
7645 /* Start for DataStoreMemory: '<S335>/Data Store Memory7' */
7646 memset(&((real_T *)ssGetDWork(S, 15))[0], 0, sizeof(real_T) << 5U);
7647
7648 /* Start for DataStoreMemory: '<S335>/Data Store Memory8' */
7649 memset(&((real_T *)ssGetDWork(S, 16))[0], 0, sizeof(real_T) << 5U);
7650
7651 /* Start for DataStoreMemory: '<S335>/Data Store Memory9' */
7652 memset(&((real_T *)ssGetDWork(S, 17))[0], 0, sizeof(real_T) << 5U);
7653
7654 /* End of Start for SubSystem: '<S9>/Trajectory planning,tracking' */
7655 /* RTW Generated Level2 S-Function Block: '<S306>/Generated S-Function15' (StickGradientDeadZone_sf) */
7656 {
7657 SimStruct *rts = ssGetSFunction(S, 167);
7658 sfcnStart(rts);
7659 if (ssGetErrorStatus(rts) != (NULL))
7660 return;
7661 }
7662
7663 /* RTW Generated Level2 S-Function Block: '<S305>/Generated S-Function15' (StickGradientDeadZone_sf) */
7664 {
7665 SimStruct *rts = ssGetSFunction(S, 168);
7666 sfcnStart(rts);
7667 if (ssGetErrorStatus(rts) != (NULL))
7668 return;
7669 }
7670
7671 /* RTW Generated Level2 S-Function Block: '<S304>/Generated S-Function15' (StickGradientDeadZone_sf) */
7672 {
7673 SimStruct *rts = ssGetSFunction(S, 169);
7674 sfcnStart(rts);
7675 if (ssGetErrorStatus(rts) != (NULL))
7676 return;
7677 }
7678
7679 /* Start for Constant: '<S13>/Constant23' */
7680 _rtB->EnLowFlag = 0;
7681
7682 /* Start for SwitchCase: '<S31>/Switch Case' */
7683 ((int8_T *)ssGetDWork(S, 505))[0] = -1;
7684
7685 /* Start for Atomic SubSystem: '<S7>/Level' */
7686
7687 /* Start for Atomic SubSystem: '<S22>/CodeReuseSubsystem' */
7688 AFCS_MODEL1_CodeReuseSubsystem_Start(S);
7689
7690 /* End of Start for SubSystem: '<S22>/CodeReuseSubsystem' */
7691
7692 /* End of Start for SubSystem: '<S7>/Level' */
7693
7694 /* ConstCode for Outport: '<Root>/PLA_L' incorporates:
7695 * Constant: '<S1>/Constant'
7696 */
7697 ((real_T *)ssGetOutputPortSignal(S, 20))[0] = 0.0;
7698
7699 /* ConstCode for Outport: '<Root>/PLA_R' incorporates:
7700 * Constant: '<S1>/Constant'
7701 */
7702 ((real_T *)ssGetOutputPortSignal(S, 21))[0] = 0.0;
7703
7704 /* ConstCode for Outport: '<Root>/N2_L' incorporates:
7705 * Constant: '<S1>/Constant'
7706 */
7707 ((real_T *)ssGetOutputPortSignal(S, 22))[0] = 0.0;
7708
7709 /* ConstCode for Outport: '<Root>/N2_R' incorporates:
7710 * Constant: '<S1>/Constant'
7711 */
7712 ((real_T *)ssGetOutputPortSignal(S, 23))[0] = 0.0;
7713
7714 /* ConstCode for DataTypeConversion: '<S24>/Data Type Conversion46' */
7715 _rtB->Quaternions[0] = (real32_T)0.0;
7716 _rtB->Quaternions[1] = (real32_T)0.0;
7717 _rtB->Quaternions[2] = (real32_T)0.0;
7718 _rtB->Quaternions[3] = (real32_T)0.0;
7719
7720 /* Previous zero-crossing state intialization */
7721 {
7722 PrevZCX_AFCS_MODEL1_T *_rtZCE;
7723 _rtZCE = ((PrevZCX_AFCS_MODEL1_T *) _ssGetPrevZCSigState(S));
7724 _rtZCE->Integrator_Reset_ZCE = UNINITIALIZED_ZCSIG;
7725 _rtZCE->Integrator_Reset_ZCE_f = UNINITIALIZED_ZCSIG;
7726 }
7727 }
7728}
7729
7730/* Outputs for root system: '<Root>' */
7731static void mdlOutputs(SimStruct *S, int_T tid)
7732{
7733 static real_T z_ECEF0;
7734 static real_T x_ECEF0;
7735 static real_T y_ECEF0;
7736 static real_T M_END2ECEF[9];
7737 static real_T Pos_ECEF[3];
7738 static real_T lata;
7739 static real_T latb;
7740 static real_T d;
7741 static real_T e;
7742 static real_T f;
7743 static real_T g;
7744 static real_T h;
7745 static real_T j;
7746 static real_T k;
7747 static real_T l;
7748 static real_T m;
7749 static real_T Pos_NED[3];
7750 static real_T d_0;
7751 static real_T e_0;
7752 static real_T f_0;
7753 static real_T g_0;
7754 static real_T h_0;
7755 static real_T i;
7756 static real_T j_0;
7757 static real_T k_0;
7758 static real_T l_0;
7759 static real_T m_0;
7760 static real_T n;
7761 static real_T o;
7762 static real_T p;
7763 static real_T q;
7764 int8_T kk1;
7765 static real32_T DXp;
7766 static real32_T DYp;
7767 static real32_T COSA;
7768 static real32_T COSB;
7769 static real_T DD0;
7770 static real_T PSDref0;
7771 static real_T Dp0;
7772 static real_T PSDp0;
7773 static real32_T PsdNorthRef;
7774 static int32_T DireFg;
7775 static real32_T Xcircl1;
7776 static real32_T Ycircl1;
7777 static real32_T Xcircl2;
7778 static real32_T Ycircl2;
7779 static real32_T DX;
7780 static real32_T DY;
7781 static real32_T DDOC;
7782 static real32_T PsiOC;
7783 static real32_T Deta;
7784 static real32_T alp1;
7785 static real32_T alp2;
7786 static real32_T Xtp1;
7787 static real32_T Ytp1;
7788 static real32_T Xtp2;
7789 static real32_T Ytp2;
7790 static real32_T XnorthTP;
7791 static real32_T YeastTP;
7792 static real32_T DD;
7793 static real32_T PsdNorthD;
7794 static real32_T Xrw;
7795 static real32_T Yrw;
7796 static real32_T b_DD;
7797 static real32_T b_PsdNorthD;
7798 static real32_T b_Xrw;
7799 static real32_T b_Yrw;
7800 static real32_T b;
7801 static real32_T c;
7802 static real32_T d_1;
7803 static real32_T e_1;
7804 static real32_T f_1;
7805 static real32_T g_1;
7806 static real32_T h_1;
7807 static real32_T i_0;
7808 static real32_T j_1;
7809 static real32_T k_1;
7810 boolean_T resetIntg;
7811 boolean_T didZcEventOccur;
7812 static real_T Lat0;
7813 static real_T Lon0;
7814 static real_T psi;
7815 static real_T waypiont_xnAll[32];
7816 static real_T waypiont_yeAll[32];
7817 static real_T waypiont_zhAll[32];
7818 static real_T waypiont_velAll[32];
7819 static real_T RminAll[32];
7820 static real_T RminAllNewCal[255];
7821 static real_T dclose;
7822 static real_T pn;
7823 static real_T pe;
7824 static real_T pd;
7825 static real_T R;
7826 static real_T unusedU2;
7827 static real_T xq1;
7828 static real_T yq1;
7829 static real_T hq1;
7830 static real_T fis;
7831 static real_T Lamda1;
7832 static real_T xq2;
7833 static real_T yq2;
7834 static real_T hq2;
7835 static real_T fi2;
7836 static real_T Lamda2;
7837 static real_T x0_dot;
7838 static real_T y0_dot;
7839 static real_T tx_dot;
7840 static real_T ty_dot;
7841 static real_T fi1sum;
7842 static real_T fi2sum;
7843 static real_T b_R;
7844 static real_T unusedU1;
7845 static real_T b_xq1;
7846 static real_T b_yq1;
7847 static real_T b_hq1;
7848 static real_T b_fis;
7849 static real_T b_Lamda1;
7850 static real_T b_xq2;
7851 static real_T b_yq2;
7852 static real_T b_hq2;
7853 static real_T b_fi2;
7854 static real_T b_Lamda2;
7855 static real_T b_x0_dot;
7856 static real_T b_y0_dot;
7857 static real_T b_tx_dot;
7858 static real_T b_ty_dot;
7859 static real_T b_fi1sum;
7860 static real_T b_fi2sum;
7861 static real_T vx_qdout[255];
7862 static real_T vy_qdout[255];
7863 static real_T xn_qdout[255];
7864 static real_T ye_qdout[255];
7865 static real_T zh_qdout[255];
7866 static real_T b_0;
7867 static real_T c_0;
7868 static real_T unusedU0;
7869 static real_T B;
7870 static real_T e_2;
7871 static real_T psic;
7872 static real_T qsn;
7873 static real_T qse;
7874 static real_T dR;
7875 static int32_T flag;
7876 static real_T qn;
7877 static real_T qe;
7878 static real_T dps;
7879 static real_T dpl;
7880 static real_T Vg;
7881 static real_T flpsic;
7882 static real_T epy;
7883 static real_T unusedU1_0;
7884 static real_T fopsic;
7885 static real_T acmd;
7886 static real_T unusedU0_0;
7887 static real_T b_fopsic;
7888 static real_T b_acmd;
7889 static real_T b_Vg;
7890 static real_T L1;
7891 static real_T Ru;
7892 static real_T psiq;
7893 static real_T delt;
7894 static real_T R_0;
7895 static real_T xt;
7896 static real_T yt;
7897 static real_T eta;
7898 static real_T T;
7899 int8_T rtAction;
7900 static real_T TrigonometricFunction4_o;
7901 static real_T j_2[9];
7902 static real_T lata_0[9];
7903 static int32_T i_1;
7904 static real_T tmp[6];
7905 static real_T tmp_0[6];
7906 static real_T d_2[9];
7907 static real_T h_2[9];
7908 static real_T x_ECEF0_0[3];
7909 static real_T rateLimiterRate;
7910 static real_T postoNextqd_idx_1;
7911 static real_T postoNextqd_idx_0;
7912 static real_T tmpForInput_idx_0;
7913 static real_T tmpForInput_idx_2;
7914 static real_T tmpForInput_idx_1;
7915 static int64_T tmp_1;
7916 static uint32_T tmp_2;
7917 static uint32_T tmp_3;
7918 static uint32_T tmp_4;
7919 static uint32_T tmp_5;
7920 static uint32_T tmp_6;
7921 static uint32_T tmp_7;
7922 static uint32_T tmp_8;
7923 uint32_T tmp_9;
7924 uint32_T qY;
7925 uint32_T tmp_a;
7926 uint32_T tmp_b;
7927 int32_T tmp_c;
7928 int32_T tmp_d;
7929 B_AFCS_MODEL1_T *_rtB;
7930 X_AFCS_MODEL1_T *_rtX;
7931 PrevZCX_AFCS_MODEL1_T *_rtZCE;
7932 _rtZCE = ((PrevZCX_AFCS_MODEL1_T *) _ssGetPrevZCSigState(S));
7933 _rtX = ((X_AFCS_MODEL1_T *) ssGetContStates(S));
7934 _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));
7935 if (1) {
7936 /* MATLAB Function: '<S1>/MATLAB Function1' incorporates:
7937 * Inport: '<Root>/lat0'
7938 * Inport: '<Root>/lon0'
7939 * Inport: '<Root>/piontNum'
7940 * Inport: '<Root>/routedata_NEH'
7941 */
7942 /* MATLAB Function 'AFCS_MODEL1/MATLAB Function1': '<S8>:1' */
7943 /* '<S8>:1:8' */
7944 /* '<S8>:1:23' */
7945 /* '<S8>:1:19' */
7946 /* 输入: */
7947 /* routedata_NEH: 航路点位置(北东高) */
7948 /* piontNum:实际航点数 */
7949 /* Lon0,Lat0,H0:起点的经纬高 */
7950 /* 输出: */
7951 /* '<S8>:1:8' */
7952 /* '<S8>:1:9' */
7953 /* '<S8>:1:10' */
7954 /* h0 = 0; */
7955 /* '<S8>:1:13' */
7956 /* '<S8>:1:14' */
7957 d = *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];
7958 AFCS_MODEL1_sind_m(&d);
7959 z_ECEF0 = 6.378137E+6 * d;
7960
7961 /* '<S8>:1:15' */
7962 e = *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];
7963 AFCS_MODEL1_cosd_c(&e);
7964 f = *((const real_T **)ssGetInputPortSignalPtrs(S, 10))[0];
7965 AFCS_MODEL1_cosd_c(&f);
7966 x_ECEF0 = 6.378137E+6 * e * f;
7967
7968 /* '<S8>:1:16' */
7969 g = *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];
7970 AFCS_MODEL1_cosd_c(&g);
7971 h = *((const real_T **)ssGetInputPortSignalPtrs(S, 10))[0];
7972 AFCS_MODEL1_sind_m(&h);
7973 y_ECEF0 = 6.378137E+6 * g * h;
7974
7975 /* '<S8>:1:18' */
7976 /* '<S8>:1:21' */
7977 /* '<S8>:1:24' */
7978 /* '<S8>:1:25' */
7979 j = -90.0 - *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];
7980 AFCS_MODEL1_cosd_c(&j);
7981 k = 90.0 + *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];
7982 AFCS_MODEL1_sind_m(&k);
7983 l = -90.0 - *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];
7984 AFCS_MODEL1_sind_m(&l);
7985 m = -90.0 - *((const real_T **)ssGetInputPortSignalPtrs(S, 11))[0];
7986 AFCS_MODEL1_cosd_c(&m);
7987 lata = *((const real_T **)ssGetInputPortSignalPtrs(S, 10))[0];
7988 AFCS_MODEL1_cosd_c(&lata);
7989 latb = *((const real_T **)ssGetInputPortSignalPtrs(S, 10))[0];
7990 AFCS_MODEL1_sind_m(&latb);
7991 TrigonometricFunction4_o = -*((const real_T **)ssGetInputPortSignalPtrs(S,
7992 10))[0];
7993 AFCS_MODEL1_sind_m(&TrigonometricFunction4_o);
7994 T = *((const real_T **)ssGetInputPortSignalPtrs(S, 10))[0];
7995 AFCS_MODEL1_cosd_c(&T);
7996 j_2[0] = j;
7997 j_2[3] = 0.0;
7998 j_2[6] = k;
7999 j_2[1] = 0.0;
8000 j_2[4] = 1.0;
8001 j_2[7] = 0.0;
8002 j_2[2] = l;
8003 j_2[5] = 0.0;
8004 j_2[8] = m;
8005 lata_0[0] = lata;
8006 lata_0[3] = latb;
8007 lata_0[6] = 0.0;
8008 lata_0[1] = TrigonometricFunction4_o;
8009 lata_0[4] = T;
8010 lata_0[7] = 0.0;
8011 lata_0[2] = 0.0;
8012 lata_0[5] = 0.0;
8013 lata_0[8] = 1.0;
8014 for (i_1 = 0; i_1 < 3; i_1++) {
8015 for (flag = 0; flag < 3; flag++) {
8016 M_END2ECEF[i_1 + 3 * flag] = 0.0;
8017 M_END2ECEF[i_1 + 3 * flag] += lata_0[3 * i_1] * j_2[flag];
8018 M_END2ECEF[i_1 + 3 * flag] += lata_0[3 * i_1 + 1] * j_2[flag + 3];
8019 M_END2ECEF[i_1 + 3 * flag] += lata_0[3 * i_1 + 2] * j_2[flag + 6];
8020 }
8021 }
8022
8023 /* '<S8>:1:27' */
8024 /* '<S8>:1:28' */
8025 memset(&_rtB->route_Lat[0], 0, 30U * sizeof(real_T));
8026 memset(&_rtB->route_Lon[0], 0, 30U * sizeof(real_T));
8027
8028 /* '<S8>:1:30' */
8029 for (flag = 0; flag < (int32_T)*((const real_T **)ssGetInputPortSignalPtrs(S,
8030 9))[0]; flag++) {
8031 /* '<S8>:1:30' */
8032 /* '<S8>:1:31' */
8033 x_ECEF0_0[0] = x_ECEF0;
8034 x_ECEF0_0[1] = y_ECEF0;
8035 x_ECEF0_0[2] = z_ECEF0;
8036 for (i_1 = 0; i_1 < 3; i_1++) {
8037 Pos_ECEF[i_1] = (-*((const real_T **)ssGetInputPortSignalPtrs(S, 8))[3 *
8038 flag + 2] * M_END2ECEF[i_1 + 6] + (*((const real_T **)
8039 ssGetInputPortSignalPtrs(S, 8))[3 * flag + 1] * M_END2ECEF[i_1 + 3] + *
8040 ((const real_T **)ssGetInputPortSignalPtrs(S, 8))[3 * flag] *
8041 M_END2ECEF[i_1])) + x_ECEF0_0[i_1];
8042 }
8043
8044 /* '<S8>:1:32' */
8045 _rtB->route_Lat[flag] = asin(Pos_ECEF[2] / (sqrt((Pos_ECEF[0] * Pos_ECEF[0]
8046 + Pos_ECEF[1] * Pos_ECEF[1]) + Pos_ECEF[2] * Pos_ECEF[2]) + 1.0E-6)) *
8047 57.295779513082323;
8048
8049 /* '<S8>:1:33' */
8050 _rtB->route_Lon[flag] = asin(Pos_ECEF[1] / (sqrt(Pos_ECEF[0] * Pos_ECEF[0]
8051 + Pos_ECEF[1] * Pos_ECEF[1]) + 1.0E-6)) * 57.295779513082323;
8052
8053 /* '<S8>:1:30' */
8054 }
8055
8056 /* '<S8>:1:35' */
8057 /* 由初始两个航点计算初始偏航角 */
8058 /* '<S8>:1:39' */
8059 /* '<S8>:1:41' */
8060 /* '<S8>:1:42' */
8061 lata = _rtB->route_Lat[0] * 0.017453292519943295;
8062
8063 /* '<S8>:1:43' */
8064 /* '<S8>:1:44' */
8065 latb = _rtB->route_Lat[1] * 0.017453292519943295;
8066
8067 /* %%%%%%%%%%%%%%%计算AB与北向夹角 */
8068 /* '<S8>:1:47' */
8069 /* '<S8>:1:48' */
8070 /* '<S8>:1:49' */
8071 /* y = u; */
8072 _rtB->PsiNorth = AFCS_MODEL1_rt_atan2d_snf((_rtB->route_Lon[1] *
8073 0.017453292519943295 - _rtB->route_Lon[0] * 0.017453292519943295) * cos
8074 ((latb + lata) * 0.5), latb - lata);
8075 for (i_1 = 0; i_1 < 30; i_1++) {
8076 _rtB->route_alt[i_1] = *((const real_T **)ssGetInputPortSignalPtrs(S, 8))
8077 [3 * i_1 + 2];
8078
8079 /* DataTypeConversion: '<S17>/Data Type Conversion' incorporates:
8080 * Inport: '<Root>/routedata_NEH'
8081 */
8082 _rtB->routeAlt[i_1] = (real32_T)_rtB->route_alt[i_1];
8083 }
8084
8085 /* End of MATLAB Function: '<S1>/MATLAB Function1' */
8086
8087 /* DataTypeConversion: '<S17>/Data Type Conversion3' incorporates:
8088 * Inport: '<Root>/piontNum'
8089 */
8090 TrigonometricFunction4_o = floor(*((const real_T **)ssGetInputPortSignalPtrs
8091 (S, 9))[0]);
8092 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
8093 {
8094 TrigonometricFunction4_o = 0.0;
8095 } else {
8096 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);
8097 }
8098
8099 _rtB->NumPoint = (int8_T)(TrigonometricFunction4_o < 0.0 ? (int32_T)(int8_T)
8100 -(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)(int8_T)(uint8_T)
8101 TrigonometricFunction4_o);
8102
8103 /* End of DataTypeConversion: '<S17>/Data Type Conversion3' */
8104
8105 /* MATLAB Function: '<S9>/MATLAB Function1' incorporates:
8106 * Constant: '<S17>/Constant6'
8107 */
8108 /* MATLAB Function 'AFCS_MODEL1/PlanningSystem_LLA/MATLAB Function1': '<S333>:1' */
8109 /* '<S333>:1:5' */
8110 memset(&_rtB->A[0], 0, 120U * sizeof(real_T));
8111
8112 /* '<S333>:1:7' */
8113 for (rtAction = 1; rtAction <= _rtB->NumPoint; rtAction++) {
8114 /* '<S333>:1:7' */
8115 /* '<S333>:1:9' */
8116 i_1 = rtAction - 1;
8117 _rtB->A[i_1 << 2] = _rtB->route_Lon[rtAction - 1];
8118 _rtB->A[1 + (i_1 << 2)] = _rtB->route_Lat[rtAction - 1];
8119 _rtB->A[2 + (i_1 << 2)] = _rtB->routeAlt[rtAction - 1];
8120 _rtB->A[3 + (i_1 << 2)] = 40.0;
8121 }
8122
8123 /* '<S333>:1:12' */
8124 _rtB->LLA0[0] = _rtB->route_Lat[0];
8125 _rtB->LLA0[1] = _rtB->route_Lon[0];
8126 _rtB->LLA0[2] = _rtB->routeAlt[0];
8127
8128 /* End of MATLAB Function: '<S9>/MATLAB Function1' */
8129
8130 /* Step: '<S1>/takeoff' */
8131 /* 纬度,经度,高度 */
8132 if (ssGetTaskTime(S,0) < 1.0) {
8133 _rtB->u2u3_a = 0.0;
8134 } else {
8135 _rtB->u2u3_a = 10.0;
8136 }
8137
8138 /* End of Step: '<S1>/takeoff' */
8139
8140 /* RelationalOperator: '<S5>/Compare' incorporates:
8141 * Constant: '<S5>/Constant'
8142 */
8143 _rtB->LogicalOperator1_b = (_rtB->u2u3_a == 10.0);
8144
8145 /* DataTypeConversion: '<S14>/Data Type Conversion' */
8146 _rtB->takeoff = _rtB->LogicalOperator1_b;
8147
8148 /* Gain: '<S14>/Gain' */
8149 _rtB->Switch_dn = 57.3 * _rtB->PsiNorth;
8150
8151 /* DataTypeConversion: '<S14>/Data Type Conversion34' */
8152 _rtB->psi_ini = (real32_T)_rtB->Switch_dn;
8153 }
8154
8155 if (ssIsSampleHit(S, 1, 0)) {
8156 /* Memory: '<S433>/Memory10' */
8157 _rtB->Memory10 = ((real32_T *)ssGetDWork(S, 270))[0];
8158
8159 /* Memory: '<S438>/Memory1' */
8160 _rtB->Memory1 = ((real32_T *)ssGetDWork(S, 271))[0];
8161
8162 /* Memory: '<S437>/Memory1' */
8163 _rtB->Memory1_k = ((real32_T *)ssGetDWork(S, 272))[0];
8164
8165 /* Memory: '<S436>/Memory1' */
8166 _rtB->Memory1_f = ((real32_T *)ssGetDWork(S, 273))[0];
8167
8168 /* Memory: '<S435>/Memory1' */
8169 _rtB->Memory1_p = ((real32_T *)ssGetDWork(S, 274))[0];
8170
8171 /* Memory: '<S434>/Memory1' */
8172 _rtB->Memory1_g = ((real32_T *)ssGetDWork(S, 275))[0];
8173
8174 /* Memory: '<S439>/Memory1' */
8175 _rtB->Memory1_e = ((real32_T *)ssGetDWork(S, 276))[0];
8176
8177 /* DataTypeConversion: '<S14>/Data Type Conversion13' incorporates:
8178 * Constant: '<S14>/Constant3'
8179 */
8180 _rtB->Alt0 = 0.0F;
8181 }
8182
8183 /* Outputs for Atomic SubSystem: '<S1>/Subsystem5' */
8184 if (ssIsSampleHit(S, 1, 0)) {
8185 /* Gain: '<S15>/Gain3' */
8186 _rtB->Gain3_be = -_rtB->Memory1_k;
8187
8188 /* Gain: '<S15>/Gain11' */
8189 _rtB->Gain11 = _rtB->Alt0;
8190
8191 /* Constant: '<S440>/Constant3' */
8192 _rtB->uvw0[0] = 0.0;
8193 _rtB->uvw0[1] = 0.0;
8194 _rtB->uvw0[2] = 0.0;
8195
8196 /* DataTypeConversion: '<S15>/Data Type Conversion1' incorporates:
8197 * Constant: '<S14>/Constant18'
8198 */
8199 _rtB->DataTypeConversion1 = -1.0;
8200 }
8201
8202 if (1) {
8203 /* Switch: '<S15>/Switch' incorporates:
8204 * Constant: '<S447>/Constant1'
8205 * Constant: '<S447>/Constant10'
8206 * Constant: '<S447>/Constant2'
8207 * Constant: '<S447>/Constant3'
8208 * Constant: '<S447>/Constant4'
8209 * Constant: '<S447>/Constant5'
8210 * Constant: '<S447>/Constant6'
8211 * Constant: '<S447>/Constant7'
8212 * Constant: '<S447>/Constant8'
8213 * Constant: '<S447>/Constant9'
8214 */
8215 if (_rtB->takeoff > 0) {
8216 _rtB->Switch_fi[0] = _rtB->Memory10;
8217 _rtB->Switch_fi[1] = _rtB->Memory1;
8218 _rtB->Switch_fi[2] = _rtB->Memory1;
8219 _rtB->Switch_fi[3] = _rtB->Memory1_k;
8220 _rtB->Switch_fi[4] = _rtB->Gain3_be;
8221 _rtB->Switch_fi[5] = _rtB->Memory1_f;
8222 _rtB->Switch_fi[6] = _rtB->Memory1_p;
8223 _rtB->Switch_fi[7] = _rtB->Memory1_g;
8224 _rtB->Switch_fi[8] = _rtB->Memory1_e;
8225 _rtB->Switch_fi[9] = _rtB->Memory1_e;
8226 } else {
8227 _rtB->Switch_fi[0] = 0.0F;
8228 _rtB->Switch_fi[1] = 0.0F;
8229 _rtB->Switch_fi[2] = 0.0F;
8230 _rtB->Switch_fi[3] = 0.0F;
8231 _rtB->Switch_fi[4] = 0.0F;
8232 _rtB->Switch_fi[5] = 0.0F;
8233 _rtB->Switch_fi[6] = 0.0F;
8234 _rtB->Switch_fi[7] = 0.0F;
8235 _rtB->Switch_fi[8] = 0.0F;
8236 _rtB->Switch_fi[9] = 0.0F;
8237 }
8238
8239 /* End of Switch: '<S15>/Switch' */
8240
8241 /* DataTypeConversion: '<S15>/Data Type Conversion4' */
8242 for (i_1 = 0; i_1 < 10; i_1++) {
8243 _rtB->DataTypeConversion4[i_1] = _rtB->Switch_fi[i_1];
8244 }
8245
8246 /* End of DataTypeConversion: '<S15>/Data Type Conversion4' */
8247
8248 /* DataTypeConversion: '<S15>/Data Type Conversion' */
8249 _rtB->DataTypeConversion = _rtB->psi_ini;
8250
8251 /* Gain: '<S442>/Unit Conversion' */
8252 _rtB->UnitConversion = 0.017453292519943295 * _rtB->DataTypeConversion;
8253
8254 /* RelationalOperator: '<S443>/Compare' incorporates:
8255 * Constant: '<S443>/Constant'
8256 */
8257 _rtB->Compare_h = (_rtB->takeoff == 1);
8258 }
8259
8260 if (ssIsSampleHit(S, 1, 0)) {
8261 /* SignalConversion: '<S15>/HiddenBuf_InsertedFor_Subsystem3_at_inport_8' */
8262 _rtB->HiddenBuf_InsertedFor_Subsystem = _rtB->Compare_h;
8263
8264 /* Outputs for Enabled SubSystem: '<S15>/Subsystem3' incorporates:
8265 * EnablePort: '<S446>/Enable'
8266 */
8267 if (ssIsMajorTimeStep(S)) {
8268 if (_rtB->HiddenBuf_InsertedFor_Subsystem) {
8269 if (!((boolean_T *)ssGetDWork(S, 614))[0]) {
8270 /* Outputs for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:
8271 * EnablePort: '<S651>/Enable'
8272 */
8273 if (ssGetTaskTime(S,1) != ssGetTStart(S)) {
8274 ssSetSolverNeedsReset(S);
8275 }
8276
8277 /* End of Outputs for SubSystem: '<S618>/GenerateGroundFM' */
8278 ((boolean_T *)ssGetDWork(S, 614))[0] = true;
8279 }
8280 } else {
8281 if (((boolean_T *)ssGetDWork(S, 614))[0]) {
8282 ssSetSolverNeedsReset(S);
8283
8284 /* Disable for If: '<S847>/If' */
8285 ((int8_T *)ssGetDWork(S, 506))[0] = -1;
8286
8287 /* Disable for If: '<S846>/If' */
8288 ((int8_T *)ssGetDWork(S, 507))[0] = -1;
8289
8290 /* Disable for Atomic SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */
8291 AFCS_MODEL1_LMainGearGeometryForcesandMoments_Disable(S,
8292 &_rtB->LMainGearGeometryForcesandMomen);
8293
8294 /* End of Disable for SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */
8295
8296 /* Disable for Atomic SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */
8297 AFCS_MODEL1_RMainGearGeometryForcesandMoments_Disable(S,
8298 &_rtB->RMainGearGeometryForcesandMomen);
8299
8300 /* End of Disable for SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */
8301
8302 /* Disable for Atomic SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */
8303 AFCS_MODEL1_NoseGearGeometryForcesandMoments_Disable(S,
8304 &_rtB->NoseGearGeometryForcesandMoment);
8305
8306 /* End of Disable for SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */
8307
8308 /* Disable for If: '<S848>/If' */
8309 ((int8_T *)ssGetDWork(S, 508))[0] = -1;
8310 ((boolean_T *)ssGetDWork(S, 614))[0] = false;
8311 }
8312 }
8313 }
8314
8315 /* End of Outputs for SubSystem: '<S15>/Subsystem3' */
8316 }
8317
8318 /* Outputs for Enabled SubSystem: '<S15>/Subsystem3' incorporates:
8319 * EnablePort: '<S446>/Enable'
8320 */
8321 if (((boolean_T *)ssGetDWork(S, 614))[0]) {
8322 if (1) {
8323 /* Integrator: '<S472>/Servo rate' */
8324 _rtB->Servorate = _rtX->Servorate_CSTATE;
8325
8326 /* Gain: '<S472>/2*zeta * wn' */
8327 _rtB->table2D_Cm_dr = 35.0 * _rtB->Servorate;
8328
8329 /* Saturate: '<S472>/Demand limits' */
8330 if (_rtB->DataTypeConversion4[1] > 30.0) {
8331 _rtB->saturation = 30.0;
8332 } else if (_rtB->DataTypeConversion4[1] < -30.0) {
8333 _rtB->saturation = -30.0;
8334 } else {
8335 _rtB->saturation = _rtB->DataTypeConversion4[1];
8336 }
8337
8338 /* End of Saturate: '<S472>/Demand limits' */
8339
8340 /* Integrator: '<S472>/Limited Deflection' */
8341 /* Limited Integrator */
8342 if (_rtX->LimitedDeflection_CSTATE >= 30.0) {
8343 if (_rtX->LimitedDeflection_CSTATE != 30.0) {
8344 _rtX->LimitedDeflection_CSTATE = 30.0;
8345 ssSetSolverNeedsReset(S);
8346 }
8347 } else {
8348 if ((_rtX->LimitedDeflection_CSTATE <= -30.0) &&
8349 (_rtX->LimitedDeflection_CSTATE != -30.0)) {
8350 _rtX->LimitedDeflection_CSTATE = -30.0;
8351 ssSetSolverNeedsReset(S);
8352 }
8353 }
8354
8355 _rtB->table2D_Cm_dfar = _rtX->LimitedDeflection_CSTATE;
8356
8357 /* End of Integrator: '<S472>/Limited Deflection' */
8358
8359 /* Sum: '<S472>/Sum3' */
8360 _rtB->saturation -= _rtB->table2D_Cm_dfar;
8361
8362 /* Saturate: '<S472>/rate limit*2*zeta//wn' */
8363 if (_rtB->saturation > 16.8) {
8364 _rtB->saturation = 16.8;
8365 } else {
8366 if (_rtB->saturation < -16.8) {
8367 _rtB->saturation = -16.8;
8368 }
8369 }
8370
8371 /* End of Saturate: '<S472>/rate limit*2*zeta//wn' */
8372
8373 /* Gain: '<S472>/wn^2' */
8374 _rtB->saturation *= 625.0;
8375
8376 /* Sum: '<S472>/Sum2' */
8377 _rtB->Sum2 = _rtB->saturation - _rtB->table2D_Cm_dr;
8378
8379 /* Integrator: '<S473>/Servo rate' */
8380 _rtB->Servorate_l = _rtX->Servorate_CSTATE_n;
8381
8382 /* Gain: '<S473>/2*zeta * wn' */
8383 _rtB->saturation = 35.0 * _rtB->Servorate_l;
8384
8385 /* Saturate: '<S473>/Demand limits' */
8386 if (_rtB->DataTypeConversion4[2] > 30.0) {
8387 _rtB->table2D_Cm_dr = 30.0;
8388 } else if (_rtB->DataTypeConversion4[2] < -30.0) {
8389 _rtB->table2D_Cm_dr = -30.0;
8390 } else {
8391 _rtB->table2D_Cm_dr = _rtB->DataTypeConversion4[2];
8392 }
8393
8394 /* End of Saturate: '<S473>/Demand limits' */
8395
8396 /* Integrator: '<S473>/Limited Deflection' */
8397 /* Limited Integrator */
8398 if (_rtX->LimitedDeflection_CSTATE_i >= 30.0) {
8399 if (_rtX->LimitedDeflection_CSTATE_i != 30.0) {
8400 _rtX->LimitedDeflection_CSTATE_i = 30.0;
8401 ssSetSolverNeedsReset(S);
8402 }
8403 } else {
8404 if ((_rtX->LimitedDeflection_CSTATE_i <= -30.0) &&
8405 (_rtX->LimitedDeflection_CSTATE_i != -30.0)) {
8406 _rtX->LimitedDeflection_CSTATE_i = -30.0;
8407 ssSetSolverNeedsReset(S);
8408 }
8409 }
8410
8411 _rtB->table2D_Cn_dfal = _rtX->LimitedDeflection_CSTATE_i;
8412
8413 /* End of Integrator: '<S473>/Limited Deflection' */
8414
8415 /* Sum: '<S473>/Sum3' */
8416 _rtB->table2D_Cm_dr -= _rtB->table2D_Cn_dfal;
8417
8418 /* Saturate: '<S473>/rate limit*2*zeta//wn' */
8419 if (_rtB->table2D_Cm_dr > 16.8) {
8420 _rtB->table2D_Cm_dr = 16.8;
8421 } else {
8422 if (_rtB->table2D_Cm_dr < -16.8) {
8423 _rtB->table2D_Cm_dr = -16.8;
8424 }
8425 }
8426
8427 /* End of Saturate: '<S473>/rate limit*2*zeta//wn' */
8428
8429 /* Gain: '<S473>/wn^2' */
8430 _rtB->table2D_Cm_dr *= 625.0;
8431
8432 /* Sum: '<S473>/Sum2' */
8433 _rtB->Sum2_o = _rtB->table2D_Cm_dr - _rtB->saturation;
8434
8435 /* Integrator: '<S474>/Servo rate' */
8436 _rtB->Servorate_lu = _rtX->Servorate_CSTATE_m;
8437
8438 /* Gain: '<S474>/2*zeta * wn' */
8439 _rtB->saturation = 35.0 * _rtB->Servorate_lu;
8440
8441 /* Saturate: '<S474>/Demand limits' */
8442 if (_rtB->DataTypeConversion4[3] > 30.0) {
8443 _rtB->table2D_Cm_dr = 30.0;
8444 } else if (_rtB->DataTypeConversion4[3] < -30.0) {
8445 _rtB->table2D_Cm_dr = -30.0;
8446 } else {
8447 _rtB->table2D_Cm_dr = _rtB->DataTypeConversion4[3];
8448 }
8449
8450 /* End of Saturate: '<S474>/Demand limits' */
8451
8452 /* Integrator: '<S474>/Limited Deflection' */
8453 /* Limited Integrator */
8454 if (_rtX->LimitedDeflection_CSTATE_m >= 30.0) {
8455 if (_rtX->LimitedDeflection_CSTATE_m != 30.0) {
8456 _rtX->LimitedDeflection_CSTATE_m = 30.0;
8457 ssSetSolverNeedsReset(S);
8458 }
8459 } else {
8460 if ((_rtX->LimitedDeflection_CSTATE_m <= -30.0) &&
8461 (_rtX->LimitedDeflection_CSTATE_m != -30.0)) {
8462 _rtX->LimitedDeflection_CSTATE_m = -30.0;
8463 ssSetSolverNeedsReset(S);
8464 }
8465 }
8466
8467 _rtB->table2D_Cn_dr = _rtX->LimitedDeflection_CSTATE_m;
8468
8469 /* End of Integrator: '<S474>/Limited Deflection' */
8470
8471 /* Sum: '<S474>/Sum3' */
8472 _rtB->table2D_Cm_dr -= _rtB->table2D_Cn_dr;
8473
8474 /* Saturate: '<S474>/rate limit*2*zeta//wn' */
8475 if (_rtB->table2D_Cm_dr > 16.8) {
8476 _rtB->table2D_Cm_dr = 16.8;
8477 } else {
8478 if (_rtB->table2D_Cm_dr < -16.8) {
8479 _rtB->table2D_Cm_dr = -16.8;
8480 }
8481 }
8482
8483 /* End of Saturate: '<S474>/rate limit*2*zeta//wn' */
8484
8485 /* Gain: '<S474>/wn^2' */
8486 _rtB->table2D_Cm_dr *= 625.0;
8487
8488 /* Sum: '<S474>/Sum2' */
8489 _rtB->Sum2_f = _rtB->table2D_Cm_dr - _rtB->saturation;
8490
8491 /* Integrator: '<S475>/Servo rate' */
8492 _rtB->Servorate_g = _rtX->Servorate_CSTATE_mi;
8493
8494 /* Gain: '<S475>/2*zeta * wn' */
8495 _rtB->saturation = 35.0 * _rtB->Servorate_g;
8496
8497 /* Saturate: '<S475>/Demand limits' */
8498 if (_rtB->DataTypeConversion4[4] > 30.0) {
8499 _rtB->table2D_Cm_dr = 30.0;
8500 } else if (_rtB->DataTypeConversion4[4] < -30.0) {
8501 _rtB->table2D_Cm_dr = -30.0;
8502 } else {
8503 _rtB->table2D_Cm_dr = _rtB->DataTypeConversion4[4];
8504 }
8505
8506 /* End of Saturate: '<S475>/Demand limits' */
8507
8508 /* Integrator: '<S475>/Limited Deflection' */
8509 /* Limited Integrator */
8510 if (_rtX->LimitedDeflection_CSTATE_f >= 30.0) {
8511 if (_rtX->LimitedDeflection_CSTATE_f != 30.0) {
8512 _rtX->LimitedDeflection_CSTATE_f = 30.0;
8513 ssSetSolverNeedsReset(S);
8514 }
8515 } else {
8516 if ((_rtX->LimitedDeflection_CSTATE_f <= -30.0) &&
8517 (_rtX->LimitedDeflection_CSTATE_f != -30.0)) {
8518 _rtX->LimitedDeflection_CSTATE_f = -30.0;
8519 ssSetSolverNeedsReset(S);
8520 }
8521 }
8522
8523 _rtB->table2D_Cn_dar = _rtX->LimitedDeflection_CSTATE_f;
8524
8525 /* End of Integrator: '<S475>/Limited Deflection' */
8526
8527 /* Sum: '<S475>/Sum3' */
8528 _rtB->table2D_Cm_dr -= _rtB->table2D_Cn_dar;
8529
8530 /* Saturate: '<S475>/rate limit*2*zeta//wn' */
8531 if (_rtB->table2D_Cm_dr > 16.8) {
8532 _rtB->table2D_Cm_dr = 16.8;
8533 } else {
8534 if (_rtB->table2D_Cm_dr < -16.8) {
8535 _rtB->table2D_Cm_dr = -16.8;
8536 }
8537 }
8538
8539 /* End of Saturate: '<S475>/rate limit*2*zeta//wn' */
8540
8541 /* Gain: '<S475>/wn^2' */
8542 _rtB->table2D_Cm_dr *= 625.0;
8543
8544 /* Sum: '<S475>/Sum2' */
8545 _rtB->Sum2_i = _rtB->table2D_Cm_dr - _rtB->saturation;
8546
8547 /* Integrator: '<S476>/Servo rate' */
8548 _rtB->Servorate_m = _rtX->Servorate_CSTATE_f;
8549
8550 /* Gain: '<S476>/2*zeta * wn' */
8551 _rtB->saturation = 35.0 * _rtB->Servorate_m;
8552
8553 /* Saturate: '<S476>/Demand limits' */
8554 if (_rtB->DataTypeConversion4[5] > 30.0) {
8555 _rtB->table2D_Cm_dr = 30.0;
8556 } else if (_rtB->DataTypeConversion4[5] < -30.0) {
8557 _rtB->table2D_Cm_dr = -30.0;
8558 } else {
8559 _rtB->table2D_Cm_dr = _rtB->DataTypeConversion4[5];
8560 }
8561
8562 /* End of Saturate: '<S476>/Demand limits' */
8563
8564 /* Integrator: '<S476>/Limited Deflection' */
8565 /* Limited Integrator */
8566 if (_rtX->LimitedDeflection_CSTATE_g >= 30.0) {
8567 if (_rtX->LimitedDeflection_CSTATE_g != 30.0) {
8568 _rtX->LimitedDeflection_CSTATE_g = 30.0;
8569 ssSetSolverNeedsReset(S);
8570 }
8571 } else {
8572 if ((_rtX->LimitedDeflection_CSTATE_g <= -30.0) &&
8573 (_rtX->LimitedDeflection_CSTATE_g != -30.0)) {
8574 _rtX->LimitedDeflection_CSTATE_g = -30.0;
8575 ssSetSolverNeedsReset(S);
8576 }
8577 }
8578
8579 _rtB->add_e = _rtX->LimitedDeflection_CSTATE_g;
8580
8581 /* End of Integrator: '<S476>/Limited Deflection' */
8582
8583 /* Sum: '<S476>/Sum3' */
8584 _rtB->table2D_Cm_dr -= _rtB->add_e;
8585
8586 /* Saturate: '<S476>/rate limit*2*zeta//wn' */
8587 if (_rtB->table2D_Cm_dr > 16.8) {
8588 _rtB->table2D_Cm_dr = 16.8;
8589 } else {
8590 if (_rtB->table2D_Cm_dr < -16.8) {
8591 _rtB->table2D_Cm_dr = -16.8;
8592 }
8593 }
8594
8595 /* End of Saturate: '<S476>/rate limit*2*zeta//wn' */
8596
8597 /* Gain: '<S476>/wn^2' */
8598 _rtB->table2D_Cm_dr *= 625.0;
8599
8600 /* Sum: '<S476>/Sum2' */
8601 _rtB->Sum2_p = _rtB->table2D_Cm_dr - _rtB->saturation;
8602
8603 /* Integrator: '<S477>/Servo rate' */
8604 _rtB->Servorate_ga = _rtX->Servorate_CSTATE_k;
8605
8606 /* Gain: '<S477>/2*zeta * wn' */
8607 _rtB->saturation = 35.0 * _rtB->Servorate_ga;
8608
8609 /* Saturate: '<S477>/Demand limits' */
8610 if (_rtB->DataTypeConversion4[6] > 30.0) {
8611 _rtB->table2D_Cm_dr = 30.0;
8612 } else if (_rtB->DataTypeConversion4[6] < -30.0) {
8613 _rtB->table2D_Cm_dr = -30.0;
8614 } else {
8615 _rtB->table2D_Cm_dr = _rtB->DataTypeConversion4[6];
8616 }
8617
8618 /* End of Saturate: '<S477>/Demand limits' */
8619
8620 /* Integrator: '<S477>/Limited Deflection' */
8621 /* Limited Integrator */
8622 if (_rtX->LimitedDeflection_CSTATE_l >= 30.0) {
8623 if (_rtX->LimitedDeflection_CSTATE_l != 30.0) {
8624 _rtX->LimitedDeflection_CSTATE_l = 30.0;
8625 ssSetSolverNeedsReset(S);
8626 }
8627 } else {
8628 if ((_rtX->LimitedDeflection_CSTATE_l <= -30.0) &&
8629 (_rtX->LimitedDeflection_CSTATE_l != -30.0)) {
8630 _rtX->LimitedDeflection_CSTATE_l = -30.0;
8631 ssSetSolverNeedsReset(S);
8632 }
8633 }
8634
8635 _rtB->Gain6_k = _rtX->LimitedDeflection_CSTATE_l;
8636
8637 /* End of Integrator: '<S477>/Limited Deflection' */
8638
8639 /* Sum: '<S477>/Sum3' */
8640 _rtB->table2D_Cm_dr -= _rtB->Gain6_k;
8641
8642 /* Saturate: '<S477>/rate limit*2*zeta//wn' */
8643 if (_rtB->table2D_Cm_dr > 16.8) {
8644 _rtB->table2D_Cm_dr = 16.8;
8645 } else {
8646 if (_rtB->table2D_Cm_dr < -16.8) {
8647 _rtB->table2D_Cm_dr = -16.8;
8648 }
8649 }
8650
8651 /* End of Saturate: '<S477>/rate limit*2*zeta//wn' */
8652
8653 /* Gain: '<S477>/wn^2' */
8654 _rtB->table2D_Cm_dr *= 625.0;
8655
8656 /* Sum: '<S477>/Sum2' */
8657 _rtB->Sum2_m = _rtB->table2D_Cm_dr - _rtB->saturation;
8658
8659 /* Integrator: '<S478>/Servo rate' */
8660 _rtB->Servorate_d = _rtX->Servorate_CSTATE_h;
8661
8662 /* Gain: '<S478>/2*zeta * wn' */
8663 _rtB->Gain6_k = 210.0 * _rtB->Servorate_d;
8664
8665 /* Saturate: '<S478>/Demand limits' */
8666 if (_rtB->DataTypeConversion4[8] > 30.0) {
8667 _rtB->saturation = 30.0;
8668 } else if (_rtB->DataTypeConversion4[8] < -30.0) {
8669 _rtB->saturation = -30.0;
8670 } else {
8671 _rtB->saturation = _rtB->DataTypeConversion4[8];
8672 }
8673
8674 /* End of Saturate: '<S478>/Demand limits' */
8675
8676 /* Integrator: '<S478>/Limited Deflection' */
8677 /* Limited Integrator */
8678 if (_rtX->LimitedDeflection_CSTATE_j >= 30.0) {
8679 if (_rtX->LimitedDeflection_CSTATE_j != 30.0) {
8680 _rtX->LimitedDeflection_CSTATE_j = 30.0;
8681 ssSetSolverNeedsReset(S);
8682 }
8683 } else {
8684 if ((_rtX->LimitedDeflection_CSTATE_j <= -30.0) &&
8685 (_rtX->LimitedDeflection_CSTATE_j != -30.0)) {
8686 _rtX->LimitedDeflection_CSTATE_j = -30.0;
8687 ssSetSolverNeedsReset(S);
8688 }
8689 }
8690
8691 _rtB->table2D_Cm_dr = _rtX->LimitedDeflection_CSTATE_j;
8692
8693 /* End of Integrator: '<S478>/Limited Deflection' */
8694
8695 /* Sum: '<S478>/Sum3' */
8696 _rtB->saturation -= _rtB->table2D_Cm_dr;
8697
8698 /* Saturate: '<S478>/rate limit*2*zeta//wn' */
8699 if (_rtB->saturation > 1.4) {
8700 _rtB->saturation = 1.4;
8701 } else {
8702 if (_rtB->saturation < -1.4) {
8703 _rtB->saturation = -1.4;
8704 }
8705 }
8706
8707 /* End of Saturate: '<S478>/rate limit*2*zeta//wn' */
8708
8709 /* Gain: '<S478>/wn^2' */
8710 _rtB->saturation *= 22500.0;
8711
8712 /* Sum: '<S478>/Sum2' */
8713 _rtB->Sum2_k = _rtB->saturation - _rtB->Gain6_k;
8714
8715 /* Integrator: '<S479>/Servo rate' */
8716 _rtB->Servorate_h = _rtX->Servorate_CSTATE_kx;
8717
8718 /* Gain: '<S479>/2*zeta * wn' */
8719 _rtB->Gain6_k = 210.0 * _rtB->Servorate_h;
8720
8721 /* Saturate: '<S479>/Demand limits' */
8722 if (_rtB->DataTypeConversion4[9] > 30.0) {
8723 _rtB->saturation = 30.0;
8724 } else if (_rtB->DataTypeConversion4[9] < -30.0) {
8725 _rtB->saturation = -30.0;
8726 } else {
8727 _rtB->saturation = _rtB->DataTypeConversion4[9];
8728 }
8729
8730 /* End of Saturate: '<S479>/Demand limits' */
8731
8732 /* Integrator: '<S479>/Limited Deflection' */
8733 /* Limited Integrator */
8734 if (_rtX->LimitedDeflection_CSTATE_la >= 30.0) {
8735 if (_rtX->LimitedDeflection_CSTATE_la != 30.0) {
8736 _rtX->LimitedDeflection_CSTATE_la = 30.0;
8737 ssSetSolverNeedsReset(S);
8738 }
8739 } else {
8740 if ((_rtX->LimitedDeflection_CSTATE_la <= -30.0) &&
8741 (_rtX->LimitedDeflection_CSTATE_la != -30.0)) {
8742 _rtX->LimitedDeflection_CSTATE_la = -30.0;
8743 ssSetSolverNeedsReset(S);
8744 }
8745 }
8746
8747 _rtB->table2D_Cm_dr = _rtX->LimitedDeflection_CSTATE_la;
8748
8749 /* End of Integrator: '<S479>/Limited Deflection' */
8750
8751 /* Sum: '<S479>/Sum3' */
8752 _rtB->saturation -= _rtB->table2D_Cm_dr;
8753
8754 /* Saturate: '<S479>/rate limit*2*zeta//wn' */
8755 if (_rtB->saturation > 1.4) {
8756 _rtB->saturation = 1.4;
8757 } else {
8758 if (_rtB->saturation < -1.4) {
8759 _rtB->saturation = -1.4;
8760 }
8761 }
8762
8763 /* End of Saturate: '<S479>/rate limit*2*zeta//wn' */
8764
8765 /* Gain: '<S479>/wn^2' */
8766 _rtB->saturation *= 22500.0;
8767
8768 /* Sum: '<S479>/Sum2' */
8769 _rtB->Sum2_mm = _rtB->saturation - _rtB->Gain6_k;
8770
8771 /* TransferFcn: '<S462>/TransferFcn1' */
8772 _rtB->Gain6_k = 0.0;
8773 _rtB->Gain6_k += 7.1710290426676231 * _rtX->TransferFcn1_CSTATE;
8774
8775 /* SignalConversion: '<S850>/TmpSignal ConversionAtsincosInport1' incorporates:
8776 * Constant: '<S440>/Constant1'
8777 * Constant: '<S440>/Constant5'
8778 */
8779 _rtB->Product2_f[0] = _rtB->UnitConversion;
8780 _rtB->Product2_f[1] = 0.0;
8781 _rtB->Product2_f[2] = 0.0;
8782
8783 /* Trigonometry: '<S850>/sincos' */
8784 TrigonometricFunction4_o = cos(_rtB->Product2_f[0]);
8785 _rtB->Product2_f[0] = sin(_rtB->Product2_f[0]);
8786 _rtB->ubvbwb[0] = TrigonometricFunction4_o;
8787 TrigonometricFunction4_o = cos(_rtB->Product2_f[1]);
8788 _rtB->Product2_f[1] = sin(_rtB->Product2_f[1]);
8789 _rtB->ubvbwb[1] = TrigonometricFunction4_o;
8790 TrigonometricFunction4_o = cos(_rtB->Product2_f[2]);
8791 _rtB->Product2_f[2] = sin(_rtB->Product2_f[2]);
8792 _rtB->ubvbwb[2] = TrigonometricFunction4_o;
8793
8794 /* Fcn: '<S850>/Fcn11' */
8795 _rtB->VectorConcatenate_f[0] = _rtB->ubvbwb[1] * _rtB->ubvbwb[0];
8796
8797 /* Fcn: '<S850>/Fcn21' */
8798 _rtB->VectorConcatenate_f[1] = _rtB->Product2_f[2] * _rtB->Product2_f[1] *
8799 _rtB->ubvbwb[0] - _rtB->ubvbwb[2] * _rtB->Product2_f[0];
8800
8801 /* Fcn: '<S850>/Fcn31' */
8802 _rtB->VectorConcatenate_f[2] = _rtB->ubvbwb[2] * _rtB->Product2_f[1] *
8803 _rtB->ubvbwb[0] + _rtB->Product2_f[2] * _rtB->Product2_f[0];
8804
8805 /* Fcn: '<S850>/Fcn12' */
8806 _rtB->VectorConcatenate_f[3] = _rtB->ubvbwb[1] * _rtB->Product2_f[0];
8807
8808 /* Fcn: '<S850>/Fcn22' */
8809 _rtB->VectorConcatenate_f[4] = _rtB->Product2_f[2] * _rtB->Product2_f[1] *
8810 _rtB->Product2_f[0] + _rtB->ubvbwb[2] * _rtB->ubvbwb[0];
8811
8812 /* Fcn: '<S850>/Fcn32' */
8813 _rtB->VectorConcatenate_f[5] = _rtB->ubvbwb[2] * _rtB->Product2_f[1] *
8814 _rtB->Product2_f[0] - _rtB->Product2_f[2] * _rtB->ubvbwb[0];
8815
8816 /* Fcn: '<S850>/Fcn13' */
8817 _rtB->VectorConcatenate_f[6] = -_rtB->Product2_f[1];
8818
8819 /* Fcn: '<S850>/Fcn23' */
8820 _rtB->VectorConcatenate_f[7] = _rtB->Product2_f[2] * _rtB->ubvbwb[1];
8821
8822 /* Fcn: '<S850>/Fcn33' */
8823 _rtB->VectorConcatenate_f[8] = _rtB->ubvbwb[2] * _rtB->ubvbwb[1];
8824 }
8825
8826 if (ssIsSampleHit(S, 1, 0)) {
8827 /* Gain: '<S872>/Unit Conversion' incorporates:
8828 * Constant: '<S14>/Constant1'
8829 * Constant: '<S14>/Constant2'
8830 */
8831 _rtB->sincos_o2_k[0] = 0.69813170079773179;
8832 _rtB->sincos_o2_k[1] = 0.52359877559829882;
8833
8834 /* Trigonometry: '<S845>/sincos' */
8835 TrigonometricFunction4_o = cos(_rtB->sincos_o2_k[0]);
8836 _rtB->sincos_o2_k[0] = sin(_rtB->sincos_o2_k[0]);
8837 _rtB->sincos_o1_h[0] = TrigonometricFunction4_o;
8838 TrigonometricFunction4_o = cos(_rtB->sincos_o2_k[1]);
8839 _rtB->sincos_o2_k[1] = sin(_rtB->sincos_o2_k[1]);
8840 _rtB->sincos_o1_h[1] = TrigonometricFunction4_o;
8841
8842 /* Product: '<S863>/u(1)*u(4)' */
8843 _rtB->Switch_ly = _rtB->sincos_o2_k[0] * _rtB->sincos_o1_h[1];
8844
8845 /* UnaryMinus: '<S863>/Unary Minus' */
8846 _rtB->VectorConcatenate[0] = -_rtB->Switch_ly;
8847
8848 /* UnaryMinus: '<S866>/Unary Minus' */
8849 _rtB->VectorConcatenate[1] = -_rtB->sincos_o2_k[1];
8850
8851 /* Product: '<S869>/u(3)*u(4)' */
8852 _rtB->Switch_ly = _rtB->sincos_o1_h[0] * _rtB->sincos_o1_h[1];
8853
8854 /* UnaryMinus: '<S869>/Unary Minus' */
8855 _rtB->VectorConcatenate[2] = -_rtB->Switch_ly;
8856
8857 /* Product: '<S864>/u(1)*u(2)' */
8858 _rtB->Switch_ly = _rtB->sincos_o2_k[0] * _rtB->sincos_o2_k[1];
8859
8860 /* UnaryMinus: '<S864>/Unary Minus' */
8861 _rtB->VectorConcatenate[3] = -_rtB->Switch_ly;
8862
8863 /* SignalConversion: '<S873>/ConcatBufferAtVector ConcatenateIn5' */
8864 _rtB->VectorConcatenate[4] = _rtB->sincos_o1_h[1];
8865
8866 /* Product: '<S870>/u(2)*u(3)' */
8867 _rtB->Switch_ly = _rtB->sincos_o2_k[1] * _rtB->sincos_o1_h[0];
8868
8869 /* UnaryMinus: '<S870>/Unary Minus' */
8870 _rtB->VectorConcatenate[5] = -_rtB->Switch_ly;
8871
8872 /* SignalConversion: '<S873>/ConcatBufferAtVector ConcatenateIn7' */
8873 _rtB->VectorConcatenate[6] = _rtB->sincos_o1_h[0];
8874
8875 /* SignalConversion: '<S873>/ConcatBufferAtVector ConcatenateIn8' incorporates:
8876 * Constant: '<S868>/Constant'
8877 */
8878 _rtB->VectorConcatenate[7] = 0.0;
8879
8880 /* UnaryMinus: '<S871>/Unary Minus' */
8881 _rtB->VectorConcatenate[8] = -_rtB->sincos_o2_k[0];
8882 }
8883
8884 if (1) {
8885 /* Product: '<S779>/Product2' */
8886 for (i_1 = 0; i_1 < 3; i_1++) {
8887 for (flag = 0; flag < 3; flag++) {
8888 _rtB->Product2[i_1 + 3 * flag] = 0.0;
8889 _rtB->Product2[i_1 + 3 * flag] += _rtB->VectorConcatenate[3 * flag] *
8890 _rtB->VectorConcatenate_f[i_1];
8891 _rtB->Product2[i_1 + 3 * flag] += _rtB->VectorConcatenate[3 * flag + 1]
8892 * _rtB->VectorConcatenate_f[i_1 + 3];
8893 _rtB->Product2[i_1 + 3 * flag] += _rtB->VectorConcatenate[3 * flag + 2]
8894 * _rtB->VectorConcatenate_f[i_1 + 6];
8895 }
8896 }
8897
8898 /* End of Product: '<S779>/Product2' */
8899
8900 /* Sum: '<S879>/Add' */
8901 _rtB->Add = (_rtB->Product2[0] + _rtB->Product2[4]) + _rtB->Product2[8];
8902
8903 /* If: '<S847>/If' */
8904 if (ssIsMajorTimeStep(S)) {
8905 rtAction = (int8_T)!(_rtB->Add > 0.0);
8906 ((int8_T *)ssGetDWork(S, 506))[0] = rtAction;
8907 } else {
8908 rtAction = ((int8_T *)ssGetDWork(S, 506))[0];
8909 }
8910
8911 switch (rtAction) {
8912 case 0:
8913 /* Outputs for IfAction SubSystem: '<S847>/Positive Trace' incorporates:
8914 * ActionPort: '<S878>/Action Port'
8915 */
8916 /* Sum: '<S878>/Sum' incorporates:
8917 * Constant: '<S878>/Constant'
8918 */
8919 _rtB->Gain1_b = _rtB->Add + 1.0;
8920
8921 /* Sqrt: '<S878>/sqrt' */
8922 _rtB->Gain1_b = sqrt(_rtB->Gain1_b);
8923
8924 /* Gain: '<S878>/Gain' */
8925 _rtB->Merge[0] = 0.5 * _rtB->Gain1_b;
8926
8927 /* Gain: '<S878>/Gain1' */
8928 _rtB->Gain1_b *= 2.0;
8929
8930 /* Sum: '<S900>/Add' */
8931 _rtB->Add_hw = _rtB->Product2[7] - _rtB->Product2[5];
8932
8933 /* Sum: '<S899>/Add' */
8934 _rtB->Add_m = _rtB->Product2[2] - _rtB->Product2[6];
8935
8936 /* Sum: '<S901>/Add' */
8937 _rtB->Add_pk = _rtB->Product2[3] - _rtB->Product2[1];
8938
8939 /* Product: '<S878>/Product' */
8940 _rtB->Merge[1] = _rtB->Add_hw / _rtB->Gain1_b;
8941 _rtB->Merge[2] = _rtB->Add_m / _rtB->Gain1_b;
8942 _rtB->Merge[3] = _rtB->Add_pk / _rtB->Gain1_b;
8943
8944 /* End of Outputs for SubSystem: '<S847>/Positive Trace' */
8945 break;
8946
8947 case 1:
8948 /* Outputs for IfAction SubSystem: '<S847>/Negative Trace' incorporates:
8949 * ActionPort: '<S877>/Action Port'
8950 */
8951 /* If: '<S877>/Find Maximum Diagonal Value' */
8952 if (ssIsMajorTimeStep(S)) {
8953 if ((_rtB->Product2[4] > _rtB->Product2[0]) && (_rtB->Product2[4] >
8954 _rtB->Product2[8])) {
8955 rtAction = 0;
8956 } else if (_rtB->Product2[8] > _rtB->Product2[0]) {
8957 rtAction = 1;
8958 } else {
8959 rtAction = 2;
8960 }
8961
8962 ((int8_T *)ssGetDWork(S, 510))[0] = rtAction;
8963 } else {
8964 rtAction = ((int8_T *)ssGetDWork(S, 510))[0];
8965 }
8966
8967 switch (rtAction) {
8968 case 0:
8969 /* Outputs for IfAction SubSystem: '<S877>/Maximum Value at DCM(2,2)' incorporates:
8970 * ActionPort: '<S881>/Action Port'
8971 */
8972 /* Sum: '<S893>/Add' incorporates:
8973 * Constant: '<S893>/Constant'
8974 */
8975 _rtB->Product_co = ((_rtB->Product2[4] - _rtB->Product2[0]) -
8976 _rtB->Product2[8]) + 1.0;
8977
8978 /* Sqrt: '<S881>/sqrt' */
8979 _rtB->Product_co = sqrt(_rtB->Product_co);
8980
8981 /* Gain: '<S881>/Gain' */
8982 _rtB->Merge[2] = 0.5 * _rtB->Product_co;
8983
8984 /* Sum: '<S891>/Add' */
8985 _rtB->Add_gh = _rtB->Product2[1] + _rtB->Product2[3];
8986
8987 /* Sum: '<S890>/Add' */
8988 _rtB->Add_er = _rtB->Product2[5] + _rtB->Product2[7];
8989
8990 /* Sum: '<S889>/Add' */
8991 _rtB->Add_f0 = _rtB->Product2[2] - _rtB->Product2[6];
8992
8993 /* Switch: '<S892>/Switch' incorporates:
8994 * Constant: '<S892>/Constant1'
8995 */
8996 if (_rtB->Product_co != 0.0) {
8997 _rtB->Switch_b5[0] = 0.5;
8998 _rtB->Switch_b5[1] = _rtB->Product_co;
8999 } else {
9000 _rtB->Switch_b5[0] = 0.0;
9001 _rtB->Switch_b5[1] = 1.0;
9002 }
9003
9004 /* End of Switch: '<S892>/Switch' */
9005
9006 /* Product: '<S892>/Product' */
9007 _rtB->Product_co = _rtB->Switch_b5[0] / _rtB->Switch_b5[1];
9008
9009 /* Product: '<S881>/Product' */
9010 _rtB->Product_e[0] = _rtB->Add_gh * _rtB->Product_co;
9011 _rtB->Product_e[1] = _rtB->Add_er * _rtB->Product_co;
9012 _rtB->Product_e[2] = _rtB->Add_f0 * _rtB->Product_co;
9013
9014 /* Gain: '<S881>/Gain1' */
9015 _rtB->Merge[1] = _rtB->Product_e[0];
9016
9017 /* Gain: '<S881>/Gain3' */
9018 _rtB->Merge[3] = _rtB->Product_e[1];
9019
9020 /* Gain: '<S881>/Gain4' */
9021 _rtB->Merge[0] = _rtB->Product_e[2];
9022
9023 /* End of Outputs for SubSystem: '<S877>/Maximum Value at DCM(2,2)' */
9024 break;
9025
9026 case 1:
9027 /* Outputs for IfAction SubSystem: '<S877>/Maximum Value at DCM(3,3)' incorporates:
9028 * ActionPort: '<S882>/Action Port'
9029 */
9030 /* Sum: '<S898>/Add' incorporates:
9031 * Constant: '<S898>/Constant'
9032 */
9033 _rtB->Product_hp = ((_rtB->Product2[8] - _rtB->Product2[0]) -
9034 _rtB->Product2[4]) + 1.0;
9035
9036 /* Sqrt: '<S882>/sqrt' */
9037 _rtB->Product_hp = sqrt(_rtB->Product_hp);
9038
9039 /* Gain: '<S882>/Gain' */
9040 _rtB->Merge[3] = 0.5 * _rtB->Product_hp;
9041
9042 /* Sum: '<S894>/Add' */
9043 _rtB->Add_i = _rtB->Product2[2] + _rtB->Product2[6];
9044
9045 /* Sum: '<S895>/Add' */
9046 _rtB->Add_kgp = _rtB->Product2[5] + _rtB->Product2[7];
9047
9048 /* Sum: '<S896>/Add' */
9049 _rtB->Add_h = _rtB->Product2[3] - _rtB->Product2[1];
9050
9051 /* Switch: '<S897>/Switch' incorporates:
9052 * Constant: '<S897>/Constant1'
9053 */
9054 if (_rtB->Product_hp != 0.0) {
9055 _rtB->Switch_n[0] = 0.5;
9056 _rtB->Switch_n[1] = _rtB->Product_hp;
9057 } else {
9058 _rtB->Switch_n[0] = 0.0;
9059 _rtB->Switch_n[1] = 1.0;
9060 }
9061
9062 /* End of Switch: '<S897>/Switch' */
9063
9064 /* Product: '<S897>/Product' */
9065 _rtB->Product_hp = _rtB->Switch_n[0] / _rtB->Switch_n[1];
9066
9067 /* Product: '<S882>/Product' */
9068 _rtB->Product_c[0] = _rtB->Add_i * _rtB->Product_hp;
9069 _rtB->Product_c[1] = _rtB->Add_kgp * _rtB->Product_hp;
9070 _rtB->Product_c[2] = _rtB->Add_h * _rtB->Product_hp;
9071
9072 /* Gain: '<S882>/Gain1' */
9073 _rtB->Merge[1] = _rtB->Product_c[0];
9074
9075 /* Gain: '<S882>/Gain2' */
9076 _rtB->Merge[2] = _rtB->Product_c[1];
9077
9078 /* Gain: '<S882>/Gain3' */
9079 _rtB->Merge[0] = _rtB->Product_c[2];
9080
9081 /* End of Outputs for SubSystem: '<S877>/Maximum Value at DCM(3,3)' */
9082 break;
9083
9084 case 2:
9085 /* Outputs for IfAction SubSystem: '<S877>/Maximum Value at DCM(1,1)' incorporates:
9086 * ActionPort: '<S880>/Action Port'
9087 */
9088 /* Sum: '<S888>/Add' incorporates:
9089 * Constant: '<S888>/Constant'
9090 */
9091 _rtB->Product_h = ((_rtB->Product2[0] - _rtB->Product2[4]) -
9092 _rtB->Product2[8]) + 1.0;
9093
9094 /* Sqrt: '<S880>/sqrt' */
9095 _rtB->Product_h = sqrt(_rtB->Product_h);
9096
9097 /* Gain: '<S880>/Gain' */
9098 _rtB->Merge[1] = 0.5 * _rtB->Product_h;
9099
9100 /* Switch: '<S887>/Switch' incorporates:
9101 * Constant: '<S887>/Constant1'
9102 */
9103 if (_rtB->Product_h != 0.0) {
9104 _rtB->Switch_gm[0] = 0.5;
9105 _rtB->Switch_gm[1] = _rtB->Product_h;
9106 } else {
9107 _rtB->Switch_gm[0] = 0.0;
9108 _rtB->Switch_gm[1] = 1.0;
9109 }
9110
9111 /* End of Switch: '<S887>/Switch' */
9112
9113 /* Product: '<S887>/Product' */
9114 _rtB->Product_h = _rtB->Switch_gm[0] / _rtB->Switch_gm[1];
9115
9116 /* Sum: '<S886>/Add' */
9117 _rtB->Add_d = _rtB->Product2[1] + _rtB->Product2[3];
9118
9119 /* Sum: '<S884>/Add' */
9120 _rtB->Add_es = _rtB->Product2[2] + _rtB->Product2[6];
9121
9122 /* Sum: '<S885>/Add' */
9123 _rtB->Add_gl = _rtB->Product2[7] - _rtB->Product2[5];
9124
9125 /* Product: '<S880>/Product' */
9126 _rtB->Product_p[0] = _rtB->Product_h * _rtB->Add_d;
9127 _rtB->Product_p[1] = _rtB->Product_h * _rtB->Add_es;
9128 _rtB->Product_p[2] = _rtB->Product_h * _rtB->Add_gl;
9129
9130 /* Gain: '<S880>/Gain1' */
9131 _rtB->Merge[2] = _rtB->Product_p[0];
9132
9133 /* Gain: '<S880>/Gain2' */
9134 _rtB->Merge[3] = _rtB->Product_p[1];
9135
9136 /* Gain: '<S880>/Gain3' */
9137 _rtB->Merge[0] = _rtB->Product_p[2];
9138
9139 /* End of Outputs for SubSystem: '<S877>/Maximum Value at DCM(1,1)' */
9140 break;
9141 }
9142
9143 /* End of If: '<S877>/Find Maximum Diagonal Value' */
9144 /* End of Outputs for SubSystem: '<S847>/Negative Trace' */
9145 break;
9146 }
9147
9148 /* End of If: '<S847>/If' */
9149
9150 /* Integrator: '<S779>/q' */
9151 if (((int_T *)ssGetDWork(S, 344))[0] != 0) {
9152 _rtX->q_CSTATE[0] = _rtB->Merge[0];
9153 _rtX->q_CSTATE[1] = _rtB->Merge[1];
9154 _rtX->q_CSTATE[2] = _rtB->Merge[2];
9155 _rtX->q_CSTATE[3] = _rtB->Merge[3];
9156 }
9157
9158 _rtB->q_j[0] = _rtX->q_CSTATE[0];
9159 _rtB->q_j[1] = _rtX->q_CSTATE[1];
9160 _rtB->q_j[2] = _rtX->q_CSTATE[2];
9161 _rtB->q_j[3] = _rtX->q_CSTATE[3];
9162
9163 /* End of Integrator: '<S779>/q' */
9164
9165 /* Product: '<S939>/Product' */
9166 _rtB->saturation = _rtB->q_j[0] * _rtB->q_j[0];
9167
9168 /* Product: '<S939>/Product1' */
9169 _rtB->table2D_Cm_dr = _rtB->q_j[1] * _rtB->q_j[1];
9170
9171 /* Product: '<S939>/Product2' */
9172 _rtB->UnitConversion_j = _rtB->q_j[2] * _rtB->q_j[2];
9173
9174 /* Product: '<S939>/Product3' */
9175 _rtB->add_d = _rtB->q_j[3] * _rtB->q_j[3];
9176
9177 /* Sum: '<S939>/Sum' */
9178 _rtB->saturation = ((_rtB->saturation + _rtB->table2D_Cm_dr) +
9179 _rtB->UnitConversion_j) + _rtB->add_d;
9180
9181 /* Sqrt: '<S938>/sqrt' */
9182 _rtB->saturation = sqrt(_rtB->saturation);
9183
9184 /* Product: '<S937>/Product' */
9185 _rtB->add_d = _rtB->q_j[0] / _rtB->saturation;
9186
9187 /* Product: '<S927>/Product3' */
9188 _rtB->UnitConversion_j = _rtB->add_d * _rtB->add_d;
9189
9190 /* Product: '<S937>/Product1' */
9191 _rtB->table2D_Cm_dr = _rtB->q_j[1] / _rtB->saturation;
9192
9193 /* Product: '<S927>/Product2' */
9194 _rtB->table2D_Cn = _rtB->table2D_Cm_dr * _rtB->table2D_Cm_dr;
9195
9196 /* Product: '<S937>/Product2' */
9197 _rtB->Product4_n = _rtB->q_j[2] / _rtB->saturation;
9198
9199 /* Product: '<S927>/Product1' */
9200 _rtB->u2rhoV2 = _rtB->Product4_n * _rtB->Product4_n;
9201
9202 /* Product: '<S937>/Product3' */
9203 _rtB->saturation = _rtB->q_j[3] / _rtB->saturation;
9204
9205 /* Product: '<S927>/Product' */
9206 _rtB->Sum3_j = _rtB->saturation * _rtB->saturation;
9207
9208 /* Sum: '<S927>/Sum' */
9209 _rtB->VectorConcatenate_n[0] = ((_rtB->UnitConversion_j + _rtB->table2D_Cn)
9210 - _rtB->u2rhoV2) - _rtB->Sum3_j;
9211
9212 /* Product: '<S930>/Product3' */
9213 _rtB->Sum3_j = _rtB->saturation * _rtB->add_d;
9214
9215 /* Product: '<S930>/Product2' */
9216 _rtB->u2rhoV2 = _rtB->table2D_Cm_dr * _rtB->Product4_n;
9217
9218 /* Sum: '<S930>/Sum' */
9219 _rtB->Sum3_j = _rtB->u2rhoV2 - _rtB->Sum3_j;
9220
9221 /* Gain: '<S930>/Gain' */
9222 _rtB->VectorConcatenate_n[1] = 2.0 * _rtB->Sum3_j;
9223
9224 /* Product: '<S933>/Product1' */
9225 _rtB->Sum3_j = _rtB->add_d * _rtB->Product4_n;
9226
9227 /* Product: '<S933>/Product2' */
9228 _rtB->u2rhoV2 = _rtB->table2D_Cm_dr * _rtB->saturation;
9229
9230 /* Sum: '<S933>/Sum' */
9231 _rtB->Sum3_j += _rtB->u2rhoV2;
9232
9233 /* Gain: '<S933>/Gain' */
9234 _rtB->VectorConcatenate_n[2] = 2.0 * _rtB->Sum3_j;
9235
9236 /* Product: '<S928>/Product3' */
9237 _rtB->Sum3_j = _rtB->saturation * _rtB->add_d;
9238
9239 /* Product: '<S928>/Product2' */
9240 _rtB->u2rhoV2 = _rtB->table2D_Cm_dr * _rtB->Product4_n;
9241
9242 /* Sum: '<S928>/Sum' */
9243 _rtB->Sum3_j += _rtB->u2rhoV2;
9244
9245 /* Gain: '<S928>/Gain' */
9246 _rtB->VectorConcatenate_n[3] = 2.0 * _rtB->Sum3_j;
9247
9248 /* Product: '<S931>/Product3' */
9249 _rtB->Sum3_j = _rtB->add_d * _rtB->add_d;
9250
9251 /* Product: '<S931>/Product2' */
9252 _rtB->u2rhoV2 = _rtB->table2D_Cm_dr * _rtB->table2D_Cm_dr;
9253
9254 /* Product: '<S931>/Product1' */
9255 _rtB->table2D_Cn = _rtB->Product4_n * _rtB->Product4_n;
9256
9257 /* Product: '<S931>/Product' */
9258 _rtB->UnitConversion_j = _rtB->saturation * _rtB->saturation;
9259
9260 /* Sum: '<S931>/Sum' */
9261 _rtB->VectorConcatenate_n[4] = ((_rtB->Sum3_j - _rtB->u2rhoV2) +
9262 _rtB->table2D_Cn) - _rtB->UnitConversion_j;
9263
9264 /* Product: '<S934>/Product1' */
9265 _rtB->Sum3_j = _rtB->add_d * _rtB->table2D_Cm_dr;
9266
9267 /* Product: '<S934>/Product2' */
9268 _rtB->u2rhoV2 = _rtB->Product4_n * _rtB->saturation;
9269
9270 /* Sum: '<S934>/Sum' */
9271 _rtB->Sum3_j = _rtB->u2rhoV2 - _rtB->Sum3_j;
9272
9273 /* Gain: '<S934>/Gain' */
9274 _rtB->VectorConcatenate_n[5] = 2.0 * _rtB->Sum3_j;
9275
9276 /* Product: '<S929>/Product1' */
9277 _rtB->Sum3_j = _rtB->add_d * _rtB->Product4_n;
9278
9279 /* Product: '<S929>/Product2' */
9280 _rtB->u2rhoV2 = _rtB->table2D_Cm_dr * _rtB->saturation;
9281
9282 /* Sum: '<S929>/Sum' */
9283 _rtB->Sum3_j = _rtB->u2rhoV2 - _rtB->Sum3_j;
9284
9285 /* Gain: '<S929>/Gain' */
9286 _rtB->VectorConcatenate_n[6] = 2.0 * _rtB->Sum3_j;
9287
9288 /* Product: '<S932>/Product1' */
9289 _rtB->Sum3_j = _rtB->add_d * _rtB->table2D_Cm_dr;
9290
9291 /* Product: '<S932>/Product2' */
9292 _rtB->u2rhoV2 = _rtB->Product4_n * _rtB->saturation;
9293
9294 /* Sum: '<S932>/Sum' */
9295 _rtB->Sum3_j += _rtB->u2rhoV2;
9296
9297 /* Gain: '<S932>/Gain' */
9298 _rtB->VectorConcatenate_n[7] = 2.0 * _rtB->Sum3_j;
9299
9300 /* Product: '<S935>/Product3' */
9301 _rtB->add_d *= _rtB->add_d;
9302
9303 /* Product: '<S935>/Product2' */
9304 _rtB->table2D_Cm_dr *= _rtB->table2D_Cm_dr;
9305
9306 /* Product: '<S935>/Product1' */
9307 _rtB->Product4_n *= _rtB->Product4_n;
9308
9309 /* Product: '<S935>/Product' */
9310 _rtB->saturation *= _rtB->saturation;
9311
9312 /* Sum: '<S935>/Sum' */
9313 _rtB->VectorConcatenate_n[8] = ((_rtB->add_d - _rtB->table2D_Cm_dr) -
9314 _rtB->Product4_n) + _rtB->saturation;
9315 }
9316
9317 if (ssIsSampleHit(S, 1, 0)) {
9318 /* Abs: '<S838>/Abs' incorporates:
9319 * Constant: '<S14>/Constant2'
9320 */
9321 _rtB->Switch_ly = 40.0;
9322
9323 /* RelationalOperator: '<S839>/Compare' incorporates:
9324 * Constant: '<S839>/Constant'
9325 */
9326 _rtB->Compare_e = (_rtB->Switch_ly > 180.0);
9327
9328 /* Switch: '<S838>/Switch' incorporates:
9329 * Constant: '<S14>/Constant2'
9330 */
9331 if (_rtB->Compare_e) {
9332 /* Bias: '<S838>/Bias' incorporates:
9333 * Constant: '<S14>/Constant2'
9334 */
9335 _rtB->MathFunction1_n = 220.0;
9336
9337 /* Math: '<S838>/Math Function1' incorporates:
9338 * Constant: '<S838>/Constant2'
9339 */
9340 _rtB->MathFunction1_n = AFCS_MODEL1_rt_modd_snf(_rtB->MathFunction1_n,
9341 360.0);
9342
9343 /* Bias: '<S838>/Bias1' */
9344 _rtB->Bias1_i = _rtB->MathFunction1_n + -180.0;
9345 _rtB->Switch_b = _rtB->Bias1_i;
9346 } else {
9347 _rtB->Switch_b = 40.0;
9348 }
9349
9350 /* End of Switch: '<S838>/Switch' */
9351
9352 /* Abs: '<S835>/Abs1' */
9353 _rtB->Abs1_e = fabs(_rtB->Switch_b);
9354
9355 /* RelationalOperator: '<S837>/Compare' incorporates:
9356 * Constant: '<S837>/Constant'
9357 */
9358 _rtB->Compare_e = (_rtB->Abs1_e > 90.0);
9359
9360 /* Switch: '<S835>/Switch' */
9361 if (_rtB->Compare_e) {
9362 /* Bias: '<S835>/Bias' */
9363 _rtB->Bias1_ac = _rtB->Abs1_e + -90.0;
9364
9365 /* Gain: '<S835>/Gain' */
9366 _rtB->Bias1_ac = -_rtB->Bias1_ac;
9367
9368 /* Bias: '<S835>/Bias1' */
9369 _rtB->Bias1_ac += 90.0;
9370
9371 /* Signum: '<S835>/Sign1' */
9372 if (_rtB->Switch_b < 0.0) {
9373 _rtB->Sign1 = -1.0;
9374 } else if (_rtB->Switch_b > 0.0) {
9375 _rtB->Sign1 = 1.0;
9376 } else if (_rtB->Switch_b == 0.0) {
9377 _rtB->Sign1 = 0.0;
9378 } else {
9379 _rtB->Sign1 = _rtB->Switch_b;
9380 }
9381
9382 /* End of Signum: '<S835>/Sign1' */
9383
9384 /* Product: '<S835>/Divide1' */
9385 _rtB->Divide1 = _rtB->Sign1 * _rtB->Bias1_ac;
9386 _rtB->Switch_ly = _rtB->Divide1;
9387 } else {
9388 _rtB->Switch_ly = _rtB->Switch_b;
9389 }
9390
9391 /* End of Switch: '<S835>/Switch' */
9392
9393 /* Gain: '<S829>/Unit Conversion' */
9394 _rtB->u2u3 = 0.017453292519943295 * _rtB->Switch_ly;
9395
9396 /* Trigonometry: '<S804>/sincos1' */
9397 _rtB->sqrt_b = sin(_rtB->u2u3);
9398 _rtB->Product2_fj = cos(_rtB->u2u3);
9399
9400 /* Product: '<S804>/Product3' incorporates:
9401 * Constant: '<S800>/Constant2'
9402 */
9403 _rtB->sincos_o1_h[0] = 0.0 * _rtB->sqrt_b;
9404 _rtB->sincos_o1_h[1] = 0.0 * _rtB->Product2_fj;
9405
9406 /* Trigonometry: '<S804>/Trigonometric Function1' */
9407 _rtB->u2u3 = tan(_rtB->u2u3);
9408
9409 /* Product: '<S804>/Product2' */
9410 _rtB->u2u3 = _rtB->u2u3 * AFCS_MODEL1_rtC(S)->Sum_bq * AFCS_MODEL1_rtC(S
9411 )->Sum_bq;
9412
9413 /* Trigonometry: '<S804>/Trigonometric Function2' incorporates:
9414 * Constant: '<S804>/f'
9415 */
9416 _rtB->u2u3 = AFCS_MODEL1_rt_atan2d_snf(_rtB->u2u3, 1.0);
9417
9418 /* Gain: '<S831>/Unit Conversion' */
9419 _rtB->Product2_fj = 57.295779513082323 * _rtB->u2u3;
9420
9421 /* Gain: '<S834>/Unit Conversion' */
9422 _rtB->Product2_fj *= 0.017453292519943295;
9423
9424 /* Trigonometry: '<S833>/Trigonometric Function' */
9425 _rtB->Product2_fj = sin(_rtB->Product2_fj);
9426
9427 /* Product: '<S833>/Product2' */
9428 _rtB->Product2_fj = _rtB->Product2_fj * _rtB->Product2_fj *
9429 AFCS_MODEL1_rtC(S)->Sum1_e;
9430
9431 /* Sum: '<S833>/Sum2' incorporates:
9432 * Constant: '<S833>/Constant'
9433 */
9434 _rtB->Product2_fj++;
9435
9436 /* Product: '<S833>/Product3' incorporates:
9437 * Constant: '<S833>/Re'
9438 */
9439 _rtB->Product2_fj = 4.0589641E+13 / _rtB->Product2_fj;
9440
9441 /* Sqrt: '<S833>/sqrt' */
9442 _rtB->Product2_fj = sqrt(_rtB->Product2_fj);
9443
9444 /* Trigonometry: '<S804>/sincos' */
9445 _rtB->sqrt_b = cos(_rtB->u2u3);
9446 _rtB->u2u3 = sin(_rtB->u2u3);
9447
9448 /* Product: '<S804>/Product1' */
9449 _rtB->sincos_o2_k[0] = _rtB->Product2_fj * _rtB->u2u3;
9450 _rtB->sincos_o2_k[1] = _rtB->Product2_fj * _rtB->sqrt_b;
9451
9452 /* Sum: '<S804>/Sum1' */
9453 _rtB->sincos_o1_h[0] += _rtB->sincos_o2_k[0];
9454 _rtB->sincos_o1_h[1] += _rtB->sincos_o2_k[1];
9455
9456 /* Trigonometry: '<S804>/Trigonometric Function3' */
9457 _rtB->Product2_fj = AFCS_MODEL1_rt_atan2d_snf(_rtB->sincos_o1_h[0],
9458 _rtB->sincos_o1_h[1]);
9459
9460 /* Gain: '<S830>/Unit Conversion' */
9461 _rtB->Product2_fj *= 57.295779513082323;
9462
9463 /* Switch: '<S805>/Switch1' incorporates:
9464 * Constant: '<S805>/Constant'
9465 * Constant: '<S805>/Constant1'
9466 */
9467 if (_rtB->Compare_e) {
9468 _rtB->sqrt_b = 180.0;
9469 } else {
9470 _rtB->sqrt_b = 0.0;
9471 }
9472
9473 /* End of Switch: '<S805>/Switch1' */
9474
9475 /* Sum: '<S805>/Sum' incorporates:
9476 * Constant: '<S14>/Constant1'
9477 */
9478 _rtB->Sum_i = _rtB->sqrt_b + 30.0;
9479
9480 /* Abs: '<S836>/Abs' */
9481 _rtB->sqrt_b = fabs(_rtB->Sum_i);
9482
9483 /* RelationalOperator: '<S840>/Compare' incorporates:
9484 * Constant: '<S840>/Constant'
9485 */
9486 _rtB->Compare_e = (_rtB->sqrt_b > 180.0);
9487
9488 /* Switch: '<S836>/Switch' */
9489 if (_rtB->Compare_e) {
9490 /* Bias: '<S836>/Bias' */
9491 _rtB->MathFunction1 = _rtB->Sum_i + 180.0;
9492
9493 /* Math: '<S836>/Math Function1' incorporates:
9494 * Constant: '<S836>/Constant2'
9495 */
9496 _rtB->MathFunction1 = AFCS_MODEL1_rt_modd_snf(_rtB->MathFunction1, 360.0);
9497
9498 /* Bias: '<S836>/Bias1' */
9499 _rtB->Bias1 = _rtB->MathFunction1 + -180.0;
9500 _rtB->sqrt_b = _rtB->Bias1;
9501 } else {
9502 _rtB->sqrt_b = _rtB->Sum_i;
9503 }
9504
9505 /* End of Switch: '<S836>/Switch' */
9506
9507 /* Gain: '<S827>/Unit Conversion' */
9508 _rtB->sincos_o1_h[0] = 0.017453292519943295 * _rtB->Product2_fj;
9509 _rtB->sincos_o1_h[1] = 0.017453292519943295 * _rtB->sqrt_b;
9510
9511 /* Trigonometry: '<S803>/sincos' */
9512 TrigonometricFunction4_o = cos(_rtB->sincos_o1_h[0]);
9513 _rtB->sincos_o1_h[0] = sin(_rtB->sincos_o1_h[0]);
9514 _rtB->sincos_o2_k[0] = TrigonometricFunction4_o;
9515 TrigonometricFunction4_o = cos(_rtB->sincos_o1_h[1]);
9516 _rtB->sincos_o1_h[1] = sin(_rtB->sincos_o1_h[1]);
9517 _rtB->sincos_o2_k[1] = TrigonometricFunction4_o;
9518
9519 /* Product: '<S818>/u(1)*u(4)' */
9520 _rtB->u2u3 = _rtB->sincos_o1_h[0] * _rtB->sincos_o2_k[1];
9521
9522 /* UnaryMinus: '<S818>/Unary Minus' */
9523 _rtB->VectorConcatenate_k[0] = -_rtB->u2u3;
9524
9525 /* UnaryMinus: '<S821>/Unary Minus' */
9526 _rtB->VectorConcatenate_k[1] = -_rtB->sincos_o1_h[1];
9527
9528 /* Product: '<S824>/u(3)*u(4)' */
9529 _rtB->u2u3 = _rtB->sincos_o2_k[0] * _rtB->sincos_o2_k[1];
9530
9531 /* UnaryMinus: '<S824>/Unary Minus' */
9532 _rtB->VectorConcatenate_k[2] = -_rtB->u2u3;
9533
9534 /* Product: '<S819>/u(1)*u(2)' */
9535 _rtB->u2u3 = _rtB->sincos_o1_h[0] * _rtB->sincos_o1_h[1];
9536
9537 /* UnaryMinus: '<S819>/Unary Minus' */
9538 _rtB->VectorConcatenate_k[3] = -_rtB->u2u3;
9539
9540 /* SignalConversion: '<S828>/ConcatBufferAtVector ConcatenateIn5' */
9541 _rtB->VectorConcatenate_k[4] = _rtB->sincos_o2_k[1];
9542
9543 /* Product: '<S825>/u(2)*u(3)' */
9544 _rtB->u2u3 = _rtB->sincos_o1_h[1] * _rtB->sincos_o2_k[0];
9545
9546 /* UnaryMinus: '<S825>/Unary Minus' */
9547 _rtB->VectorConcatenate_k[5] = -_rtB->u2u3;
9548
9549 /* SignalConversion: '<S828>/ConcatBufferAtVector ConcatenateIn7' */
9550 _rtB->VectorConcatenate_k[6] = _rtB->sincos_o2_k[0];
9551
9552 /* SignalConversion: '<S828>/ConcatBufferAtVector ConcatenateIn8' incorporates:
9553 * Constant: '<S823>/Constant'
9554 */
9555 _rtB->VectorConcatenate_k[7] = 0.0;
9556
9557 /* UnaryMinus: '<S826>/Unary Minus' */
9558 _rtB->VectorConcatenate_k[8] = -_rtB->sincos_o1_h[0];
9559
9560 /* Math: '<S800>/Math Function1' */
9561 for (i_1 = 0; i_1 < 3; i_1++) {
9562 _rtB->MathFunction[3 * i_1] = _rtB->VectorConcatenate_k[i_1];
9563 _rtB->MathFunction[1 + 3 * i_1] = _rtB->VectorConcatenate_k[i_1 + 3];
9564 _rtB->MathFunction[2 + 3 * i_1] = _rtB->VectorConcatenate_k[i_1 + 6];
9565 }
9566
9567 /* End of Math: '<S800>/Math Function1' */
9568
9569 /* Gain: '<S841>/Unit Conversion' */
9570 _rtB->Product2_fj *= 0.017453292519943295;
9571
9572 /* Trigonometry: '<S806>/Trigonometric Function' */
9573 _rtB->Product2_fj = sin(_rtB->Product2_fj);
9574
9575 /* Product: '<S806>/Product2' */
9576 _rtB->Product2_fj = _rtB->Product2_fj * _rtB->Product2_fj *
9577 AFCS_MODEL1_rtC(S)->Sum1_b;
9578
9579 /* Sum: '<S806>/Sum2' incorporates:
9580 * Constant: '<S806>/Constant'
9581 */
9582 _rtB->Product2_fj++;
9583
9584 /* Product: '<S806>/Product3' incorporates:
9585 * Constant: '<S806>/Re'
9586 */
9587 _rtB->Product2_fj = 4.0589641E+13 / _rtB->Product2_fj;
9588
9589 /* Sqrt: '<S806>/sqrt' */
9590 _rtB->Product2_fj = sqrt(_rtB->Product2_fj);
9591
9592 /* UnaryMinus: '<S800>/Unary Minus' */
9593 _rtB->Product2_fj = -_rtB->Product2_fj;
9594
9595 /* SignalConversion: '<S800>/TmpSignal ConversionAtProduct1Inport2' incorporates:
9596 * Constant: '<S800>/Constant'
9597 */
9598 _rtB->TmpSignalConversionAtProductInp[0] = 0.0;
9599 _rtB->TmpSignalConversionAtProductInp[1] = 0.0;
9600 _rtB->TmpSignalConversionAtProductInp[2] = _rtB->Product2_fj;
9601
9602 /* Product: '<S800>/Product1' */
9603 for (i_1 = 0; i_1 < 3; i_1++) {
9604 _rtB->Product1_ec[i_1] = 0.0;
9605 _rtB->Product1_ec[i_1] += _rtB->MathFunction[i_1] *
9606 _rtB->TmpSignalConversionAtProductInp[0];
9607 _rtB->Product1_ec[i_1] += _rtB->MathFunction[i_1 + 3] *
9608 _rtB->TmpSignalConversionAtProductInp[1];
9609 _rtB->Product1_ec[i_1] += _rtB->MathFunction[i_1 + 6] *
9610 _rtB->TmpSignalConversionAtProductInp[2];
9611 }
9612
9613 /* End of Product: '<S800>/Product1' */
9614
9615 /* Gain: '<S816>/Unit Conversion' */
9616 _rtB->sincos_o1_h[0] = 0.017453292519943295 * _rtB->Switch_ly;
9617 _rtB->sincos_o1_h[1] = 0.017453292519943295 * _rtB->sqrt_b;
9618
9619 /* Trigonometry: '<S802>/sincos' */
9620 TrigonometricFunction4_o = cos(_rtB->sincos_o1_h[0]);
9621 _rtB->sincos_o1_h[0] = sin(_rtB->sincos_o1_h[0]);
9622 _rtB->sincos_o2_k[0] = TrigonometricFunction4_o;
9623 TrigonometricFunction4_o = cos(_rtB->sincos_o1_h[1]);
9624 _rtB->sincos_o1_h[1] = sin(_rtB->sincos_o1_h[1]);
9625 _rtB->sincos_o2_k[1] = TrigonometricFunction4_o;
9626
9627 /* Product: '<S807>/u(1)*u(4)' */
9628 _rtB->Product2_fj = _rtB->sincos_o1_h[0] * _rtB->sincos_o2_k[1];
9629
9630 /* UnaryMinus: '<S807>/Unary Minus' */
9631 _rtB->VectorConcatenate_be[0] = -_rtB->Product2_fj;
9632
9633 /* UnaryMinus: '<S810>/Unary Minus' */
9634 _rtB->VectorConcatenate_be[1] = -_rtB->sincos_o1_h[1];
9635
9636 /* Product: '<S813>/u(3)*u(4)' */
9637 _rtB->Product2_fj = _rtB->sincos_o2_k[0] * _rtB->sincos_o2_k[1];
9638
9639 /* UnaryMinus: '<S813>/Unary Minus' */
9640 _rtB->VectorConcatenate_be[2] = -_rtB->Product2_fj;
9641
9642 /* Product: '<S808>/u(1)*u(2)' */
9643 _rtB->Product2_fj = _rtB->sincos_o1_h[0] * _rtB->sincos_o1_h[1];
9644
9645 /* UnaryMinus: '<S808>/Unary Minus' */
9646 _rtB->VectorConcatenate_be[3] = -_rtB->Product2_fj;
9647
9648 /* SignalConversion: '<S817>/ConcatBufferAtVector ConcatenateIn5' */
9649 _rtB->VectorConcatenate_be[4] = _rtB->sincos_o2_k[1];
9650
9651 /* Product: '<S814>/u(2)*u(3)' */
9652 _rtB->Product2_fj = _rtB->sincos_o1_h[1] * _rtB->sincos_o2_k[0];
9653
9654 /* UnaryMinus: '<S814>/Unary Minus' */
9655 _rtB->VectorConcatenate_be[5] = -_rtB->Product2_fj;
9656
9657 /* SignalConversion: '<S817>/ConcatBufferAtVector ConcatenateIn7' */
9658 _rtB->VectorConcatenate_be[6] = _rtB->sincos_o2_k[0];
9659
9660 /* SignalConversion: '<S817>/ConcatBufferAtVector ConcatenateIn8' incorporates:
9661 * Constant: '<S812>/Constant'
9662 */
9663 _rtB->VectorConcatenate_be[7] = 0.0;
9664
9665 /* UnaryMinus: '<S815>/Unary Minus' */
9666 _rtB->VectorConcatenate_be[8] = -_rtB->sincos_o1_h[0];
9667
9668 /* Math: '<S800>/Math Function' */
9669 for (i_1 = 0; i_1 < 3; i_1++) {
9670 _rtB->MathFunction[3 * i_1] = _rtB->VectorConcatenate_be[i_1];
9671 _rtB->MathFunction[1 + 3 * i_1] = _rtB->VectorConcatenate_be[i_1 + 3];
9672 _rtB->MathFunction[2 + 3 * i_1] = _rtB->VectorConcatenate_be[i_1 + 6];
9673 }
9674
9675 /* End of Math: '<S800>/Math Function' */
9676
9677 /* UnaryMinus: '<S800>/Unary Minus1' */
9678 _rtB->Product2_fj = -_rtB->Gain11;
9679
9680 /* SignalConversion: '<S800>/TmpSignal ConversionAtProductInport2' incorporates:
9681 * Constant: '<S800>/Constant1'
9682 */
9683 _rtB->TmpSignalConversionAtProductInp[0] = 0.0;
9684 _rtB->TmpSignalConversionAtProductInp[1] = 0.0;
9685 _rtB->TmpSignalConversionAtProductInp[2] = _rtB->Product2_fj;
9686 for (i_1 = 0; i_1 < 3; i_1++) {
9687 /* Product: '<S800>/Product' */
9688 _rtB->Product_g5[i_1] = 0.0;
9689 _rtB->Product_g5[i_1] += _rtB->MathFunction[i_1] *
9690 _rtB->TmpSignalConversionAtProductInp[0];
9691 _rtB->Product_g5[i_1] += _rtB->MathFunction[i_1 + 3] *
9692 _rtB->TmpSignalConversionAtProductInp[1];
9693 _rtB->Product_g5[i_1] += _rtB->MathFunction[i_1 + 6] *
9694 _rtB->TmpSignalConversionAtProductInp[2];
9695
9696 /* Sum: '<S800>/Sum' */
9697 _rtB->Sum_k[i_1] = _rtB->Product1_ec[i_1] + _rtB->Product_g5[i_1];
9698 }
9699 }
9700
9701 if (1) {
9702 /* Integrator: '<S778>/p1' */
9703 if (((int_T *)ssGetDWork(S, 345))[0] != 0) {
9704 _rtX->p1_CSTATE[0] = _rtB->Sum_k[0];
9705 _rtX->p1_CSTATE[1] = _rtB->Sum_k[1];
9706 _rtX->p1_CSTATE[2] = _rtB->Sum_k[2];
9707 }
9708
9709 _rtB->p1[0] = _rtX->p1_CSTATE[0];
9710 _rtB->p1[1] = _rtX->p1_CSTATE[1];
9711 _rtB->p1[2] = _rtX->p1_CSTATE[2];
9712
9713 /* End of Integrator: '<S778>/p1' */
9714
9715 /* Product: '<S785>/Product2' */
9716 _rtB->Sum3_j = _rtB->p1[0] * _rtB->p1[0];
9717
9718 /* Product: '<S785>/Product3' */
9719 _rtB->u2rhoV2 = _rtB->p1[1] * _rtB->p1[1];
9720
9721 /* Sum: '<S785>/Sum2' */
9722 _rtB->Sum3_j += _rtB->u2rhoV2;
9723
9724 /* Sqrt: '<S785>/sqrt' */
9725 _rtB->sqrt_m = sqrt(_rtB->Sum3_j);
9726 }
9727
9728 if (ssIsSampleHit(S, 1, 0)) {
9729 /* Outputs for Iterator SubSystem: '<S774>/While Iterator Subsystem' incorporates:
9730 * WhileIterator: '<S787>/While Iterator'
9731 */
9732 rtAction = 1;
9733
9734 /* End of Outputs for SubSystem: '<S774>/While Iterator Subsystem' */
9735 do {
9736 _rtB->WhileIterator = rtAction;
9737 _rtB->Switch_na = ((real_T *)ssGetDWork(S, 11))[0];
9738 if (!(_rtB->WhileIterator > 0)) {
9739 _rtB->Product2_in = AFCS_MODEL1_rtC(S)->Sum1_k * _rtB->sqrt_m;
9740 _rtB->TrigonometricFunction3 = AFCS_MODEL1_rt_atan2d_snf(_rtB->p1[2],
9741 _rtB->Product2_in);
9742 _rtB->Switch_na = _rtB->TrigonometricFunction3;
9743 }
9744
9745 _rtB->sincos_o2_c = sin(_rtB->Switch_na);
9746 _rtB->Product3_e = cos(_rtB->Switch_na);
9747 _rtB->sincos_o2_c *= AFCS_MODEL1_rtC(S)->Product2_o * AFCS_MODEL1_rtC(S
9748 )->Product1_l5 * _rtB->sincos_o2_c * _rtB->sincos_o2_c;
9749 _rtB->sincos_o2_c += _rtB->p1[2];
9750 _rtB->Product3_e = _rtB->Product3_e * _rtB->Product3_e *
9751 _rtB->Product3_e * AFCS_MODEL1_rtC(S)->Sum1_o * 6.371E+6;
9752 _rtB->Product3_e = _rtB->sqrt_m - _rtB->Product3_e;
9753 _rtB->TrigonometricFunction4 = AFCS_MODEL1_rt_atan2d_snf
9754 (_rtB->sincos_o2_c, _rtB->Product3_e);
9755 _rtB->Product3_e = sin(_rtB->TrigonometricFunction4);
9756 _rtB->sincos_o2_c = cos(_rtB->TrigonometricFunction4);
9757 _rtB->Product3_e *= AFCS_MODEL1_rtC(S)->Sum1_k;
9758 TrigonometricFunction4_o = AFCS_MODEL1_rt_atan2d_snf(_rtB->Product3_e,
9759 _rtB->sincos_o2_c);
9760 _rtB->RelationalOperator = (_rtB->Switch_na != TrigonometricFunction4_o);
9761 ((real_T *)ssGetDWork(S, 11))[0] = TrigonometricFunction4_o;
9762 rtAction++;
9763 } while (_rtB->RelationalOperator && (rtAction <= 5));
9764 }
9765
9766 if (1) {
9767 /* Trigonometry: '<S774>/Trigonometric Function2' */
9768 _rtB->Sum3_j = AFCS_MODEL1_rt_atan2d_snf(_rtB->p1[1], _rtB->p1[0]);
9769
9770 /* Gain: '<S784>/Unit Conversion' */
9771 _rtB->UnitConversion_c4[0] = 57.295779513082323 *
9772 _rtB->TrigonometricFunction4;
9773 _rtB->UnitConversion_c4[1] = 57.295779513082323 * _rtB->Sum3_j;
9774
9775 /* Gain: '<S861>/Unit Conversion' */
9776 _rtB->sincos_o2_h[0] = 0.017453292519943295 * _rtB->UnitConversion_c4[0];
9777
9778 /* Trigonometry: '<S844>/sincos' */
9779 TrigonometricFunction4_o = cos(_rtB->sincos_o2_h[0]);
9780 _rtB->sincos_o2_h[0] = sin(_rtB->sincos_o2_h[0]);
9781 _rtB->sincos_o1_m[0] = TrigonometricFunction4_o;
9782
9783 /* Gain: '<S861>/Unit Conversion' */
9784 _rtB->sincos_o2_h[1] = 0.017453292519943295 * _rtB->UnitConversion_c4[1];
9785
9786 /* Trigonometry: '<S844>/sincos' */
9787 TrigonometricFunction4_o = cos(_rtB->sincos_o2_h[1]);
9788 _rtB->sincos_o2_h[1] = sin(_rtB->sincos_o2_h[1]);
9789 _rtB->sincos_o1_m[1] = TrigonometricFunction4_o;
9790
9791 /* Product: '<S852>/u(1)*u(4)' */
9792 _rtB->Sum3_j = _rtB->sincos_o2_h[0] * _rtB->sincos_o1_m[1];
9793
9794 /* UnaryMinus: '<S852>/Unary Minus' */
9795 _rtB->VectorConcatenate_nn[0] = -_rtB->Sum3_j;
9796
9797 /* UnaryMinus: '<S855>/Unary Minus' */
9798 _rtB->VectorConcatenate_nn[1] = -_rtB->sincos_o2_h[1];
9799
9800 /* Product: '<S858>/u(3)*u(4)' */
9801 _rtB->Sum3_j = _rtB->sincos_o1_m[0] * _rtB->sincos_o1_m[1];
9802
9803 /* UnaryMinus: '<S858>/Unary Minus' */
9804 _rtB->VectorConcatenate_nn[2] = -_rtB->Sum3_j;
9805
9806 /* Product: '<S853>/u(1)*u(2)' */
9807 _rtB->Sum3_j = _rtB->sincos_o2_h[0] * _rtB->sincos_o2_h[1];
9808
9809 /* UnaryMinus: '<S853>/Unary Minus' */
9810 _rtB->VectorConcatenate_nn[3] = -_rtB->Sum3_j;
9811
9812 /* SignalConversion: '<S862>/ConcatBufferAtVector ConcatenateIn5' */
9813 _rtB->VectorConcatenate_nn[4] = _rtB->sincos_o1_m[1];
9814
9815 /* Product: '<S859>/u(2)*u(3)' */
9816 _rtB->Sum3_j = _rtB->sincos_o2_h[1] * _rtB->sincos_o1_m[0];
9817
9818 /* UnaryMinus: '<S859>/Unary Minus' */
9819 _rtB->VectorConcatenate_nn[5] = -_rtB->Sum3_j;
9820
9821 /* SignalConversion: '<S862>/ConcatBufferAtVector ConcatenateIn7' */
9822 _rtB->VectorConcatenate_nn[6] = _rtB->sincos_o1_m[0];
9823
9824 /* SignalConversion: '<S862>/ConcatBufferAtVector ConcatenateIn8' incorporates:
9825 * Constant: '<S857>/Constant'
9826 */
9827 _rtB->VectorConcatenate_nn[7] = 0.0;
9828
9829 /* UnaryMinus: '<S860>/Unary Minus' */
9830 _rtB->VectorConcatenate_nn[8] = -_rtB->sincos_o2_h[0];
9831
9832 /* Math: '<S779>/Math Function2' */
9833 for (i_1 = 0; i_1 < 3; i_1++) {
9834 _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_nn[i_1];
9835 _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_nn[i_1 + 3];
9836 _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_nn[i_1 + 6];
9837 }
9838
9839 /* End of Math: '<S779>/Math Function2' */
9840
9841 /* Product: '<S779>/Product4' */
9842 for (i_1 = 0; i_1 < 3; i_1++) {
9843 for (flag = 0; flag < 3; flag++) {
9844 _rtB->Product4[i_1 + 3 * flag] = 0.0;
9845 _rtB->Product4[i_1 + 3 * flag] += _rtB->MathFunction1_a[3 * flag] *
9846 _rtB->VectorConcatenate_n[i_1];
9847 _rtB->Product4[i_1 + 3 * flag] += _rtB->MathFunction1_a[3 * flag + 1] *
9848 _rtB->VectorConcatenate_n[i_1 + 3];
9849 _rtB->Product4[i_1 + 3 * flag] += _rtB->MathFunction1_a[3 * flag + 2] *
9850 _rtB->VectorConcatenate_n[i_1 + 6];
9851 }
9852 }
9853
9854 /* End of Product: '<S779>/Product4' */
9855
9856 /* SignalConversion: '<S615>/TmpSignal ConversionAtProduct2Inport2' incorporates:
9857 * Inport: '<Root>/Vx_wind'
9858 * Inport: '<Root>/Vy_wind'
9859 * Inport: '<Root>/Vz_wind'
9860 */
9861 _rtB->ubvbwb[0] = *((const real_T **)ssGetInputPortSignalPtrs(S, 5))[0];
9862 _rtB->ubvbwb[1] = *((const real_T **)ssGetInputPortSignalPtrs(S, 6))[0];
9863 _rtB->ubvbwb[2] = *((const real_T **)ssGetInputPortSignalPtrs(S, 7))[0];
9864
9865 /* Product: '<S615>/Product2' */
9866 for (i_1 = 0; i_1 < 3; i_1++) {
9867 _rtB->Product2_i[i_1] = 0.0;
9868 _rtB->Product2_i[i_1] += _rtB->Product4[i_1] * _rtB->ubvbwb[0];
9869 _rtB->Product2_i[i_1] += _rtB->Product4[i_1 + 3] * _rtB->ubvbwb[1];
9870 _rtB->Product2_i[i_1] += _rtB->Product4[i_1 + 6] * _rtB->ubvbwb[2];
9871 }
9872
9873 /* End of Product: '<S615>/Product2' */
9874 }
9875
9876 if (ssIsSampleHit(S, 1, 0)) {
9877 /* Trigonometry: '<S786>/sincos' */
9878 _rtB->sincos_o1 = sin(_rtB->TrigonometricFunction4);
9879 _rtB->sincos_o2 = cos(_rtB->TrigonometricFunction4);
9880
9881 /* Product: '<S786>/Product2' */
9882 _rtB->Product2_fj = _rtB->sincos_o1 * AFCS_MODEL1_rtC(S)->Sum1_o;
9883
9884 /* Product: '<S786>/Product1' */
9885 _rtB->sqrt_b = _rtB->Product2_fj * _rtB->sincos_o1;
9886
9887 /* Sum: '<S786>/Sum' incorporates:
9888 * Constant: '<S786>/f'
9889 */
9890 _rtB->sqrt_b = 1.0 - _rtB->sqrt_b;
9891
9892 /* Sqrt: '<S786>/sqrt' */
9893 _rtB->sqrt_b = sqrt(_rtB->sqrt_b);
9894
9895 /* Product: '<S786>/Product3' incorporates:
9896 * Constant: '<S786>/Constant1'
9897 */
9898 _rtB->Product3 = 6.371E+6 / _rtB->sqrt_b;
9899
9900 /* Product: '<S786>/Product4' */
9901 _rtB->Product4_k = _rtB->Product2_fj * _rtB->Product3;
9902 }
9903
9904 if (1) {
9905 /* Sum: '<S786>/Sum2' */
9906 _rtB->Sum3_j = _rtB->p1[2] + _rtB->Product4_k;
9907
9908 /* Product: '<S786>/Product5' */
9909 _rtB->Sum3_j *= _rtB->sincos_o1;
9910
9911 /* Product: '<S786>/Product6' */
9912 _rtB->u2rhoV2 = _rtB->sincos_o2 * _rtB->sqrt_m;
9913
9914 /* Sum: '<S786>/Sum3' */
9915 _rtB->Sum3_j = (_rtB->Sum3_j - _rtB->Product3) + _rtB->u2rhoV2;
9916
9917 /* Gain: '<S446>/Gain2' */
9918 _rtB->Alt = _rtB->Sum3_j;
9919
9920 /* Sum: '<S446>/Sum2' */
9921 _rtB->u2rhoV2 = _rtB->Alt - _rtB->DataTypeConversion1;
9922
9923 /* Switch: '<S465>/Switch1' incorporates:
9924 * Saturate: '<S465>/Saturation'
9925 */
9926 if (_rtB->u2rhoV2 > 30.0) {
9927 tmpForInput_idx_0 = _rtB->Product2_i[0];
9928 tmpForInput_idx_1 = _rtB->Product2_i[1];
9929 tmpForInput_idx_2 = _rtB->Product2_i[2];
9930 } else {
9931 if (_rtB->Product2_i[0] > 10.0) {
9932 /* Saturate: '<S465>/Saturation' */
9933 _rtB->Saturation_b[0] = 10.0;
9934 } else if (_rtB->Product2_i[0] < -10.0) {
9935 /* Saturate: '<S465>/Saturation' */
9936 _rtB->Saturation_b[0] = -10.0;
9937 } else {
9938 /* Saturate: '<S465>/Saturation' */
9939 _rtB->Saturation_b[0] = _rtB->Product2_i[0];
9940 }
9941
9942 tmpForInput_idx_0 = _rtB->Saturation_b[0];
9943
9944 /* Saturate: '<S465>/Saturation' */
9945 if (_rtB->Product2_i[1] > 10.0) {
9946 _rtB->Saturation_b[1] = 10.0;
9947 } else if (_rtB->Product2_i[1] < -10.0) {
9948 _rtB->Saturation_b[1] = -10.0;
9949 } else {
9950 _rtB->Saturation_b[1] = _rtB->Product2_i[1];
9951 }
9952
9953 tmpForInput_idx_1 = _rtB->Saturation_b[1];
9954
9955 /* Saturate: '<S465>/Saturation' */
9956 if (_rtB->Product2_i[2] > 10.0) {
9957 _rtB->Saturation_b[2] = 10.0;
9958 } else if (_rtB->Product2_i[2] < -10.0) {
9959 _rtB->Saturation_b[2] = -10.0;
9960 } else {
9961 _rtB->Saturation_b[2] = _rtB->Product2_i[2];
9962 }
9963
9964 tmpForInput_idx_2 = _rtB->Saturation_b[2];
9965 }
9966
9967 /* End of Switch: '<S465>/Switch1' */
9968
9969 /* RateLimiter: '<S465>/Rate Limiter' */
9970 if (((real_T *)ssGetDWork(S, 3))[0] == (rtInf)) {
9971 _rtB->RateLimiter[0] = tmpForInput_idx_0;
9972 _rtB->RateLimiter[1] = tmpForInput_idx_1;
9973 _rtB->RateLimiter[2] = tmpForInput_idx_2;
9974 } else {
9975 T = ssGetTaskTime(S,0) - ((real_T *)ssGetDWork(S, 3))[0];
9976 TrigonometricFunction4_o = T * 2.0;
9977 T *= -2.0;
9978 rateLimiterRate = tmpForInput_idx_0 - ((real_T *)ssGetDWork(S, 2))[0];
9979 if (rateLimiterRate > TrigonometricFunction4_o) {
9980 _rtB->RateLimiter[0] = ((real_T *)ssGetDWork(S, 2))[0] +
9981 TrigonometricFunction4_o;
9982 } else if (rateLimiterRate < T) {
9983 _rtB->RateLimiter[0] = ((real_T *)ssGetDWork(S, 2))[0] + T;
9984 } else {
9985 _rtB->RateLimiter[0] = tmpForInput_idx_0;
9986 }
9987
9988 rateLimiterRate = tmpForInput_idx_1 - ((real_T *)ssGetDWork(S, 2))[1];
9989 if (rateLimiterRate > TrigonometricFunction4_o) {
9990 _rtB->RateLimiter[1] = ((real_T *)ssGetDWork(S, 2))[1] +
9991 TrigonometricFunction4_o;
9992 } else if (rateLimiterRate < T) {
9993 _rtB->RateLimiter[1] = ((real_T *)ssGetDWork(S, 2))[1] + T;
9994 } else {
9995 _rtB->RateLimiter[1] = tmpForInput_idx_1;
9996 }
9997
9998 rateLimiterRate = tmpForInput_idx_2 - ((real_T *)ssGetDWork(S, 2))[2];
9999 if (rateLimiterRate > TrigonometricFunction4_o) {
10000 _rtB->RateLimiter[2] = ((real_T *)ssGetDWork(S, 2))[2] +
10001 TrigonometricFunction4_o;
10002 } else if (rateLimiterRate < T) {
10003 _rtB->RateLimiter[2] = ((real_T *)ssGetDWork(S, 2))[2] + T;
10004 } else {
10005 _rtB->RateLimiter[2] = tmpForInput_idx_2;
10006 }
10007 }
10008
10009 /* End of RateLimiter: '<S465>/Rate Limiter' */
10010
10011 /* Integrator: '<S780>/ub,vb,wb' */
10012 if (((int_T *)ssGetDWork(S, 346))[0] != 0) {
10013 _rtX->ubvbwb_CSTATE[0] = _rtB->uvw0[0];
10014 _rtX->ubvbwb_CSTATE[1] = _rtB->uvw0[1];
10015 _rtX->ubvbwb_CSTATE[2] = _rtB->uvw0[2];
10016 }
10017
10018 _rtB->ubvbwb[0] = _rtX->ubvbwb_CSTATE[0];
10019
10020 /* Gain: '<S782>/Unit Conversion' */
10021 _rtB->UnitConversion_o[0] = _rtB->ubvbwb[0];
10022
10023 /* Sum: '<S465>/Sum' */
10024 _rtB->Sum_m[0] = _rtB->UnitConversion_o[0] - _rtB->RateLimiter[0];
10025
10026 /* Integrator: '<S780>/ub,vb,wb' */
10027 _rtB->ubvbwb[1] = _rtX->ubvbwb_CSTATE[1];
10028
10029 /* Gain: '<S782>/Unit Conversion' */
10030 _rtB->UnitConversion_o[1] = _rtB->ubvbwb[1];
10031
10032 /* Sum: '<S465>/Sum' */
10033 _rtB->Sum_m[1] = _rtB->UnitConversion_o[1] - _rtB->RateLimiter[1];
10034
10035 /* Integrator: '<S780>/ub,vb,wb' */
10036 _rtB->ubvbwb[2] = _rtX->ubvbwb_CSTATE[2];
10037
10038 /* Gain: '<S782>/Unit Conversion' */
10039 _rtB->UnitConversion_o[2] = _rtB->ubvbwb[2];
10040
10041 /* Sum: '<S465>/Sum' */
10042 _rtB->Sum_m[2] = _rtB->UnitConversion_o[2] - _rtB->RateLimiter[2];
10043
10044 /* Product: '<S609>/Product' */
10045 _rtB->u2rhoV2 = _rtB->Sum_m[0] * _rtB->Sum_m[0];
10046
10047 /* Product: '<S609>/Product1' */
10048 _rtB->Product4_n = _rtB->Sum_m[1] * _rtB->Sum_m[1];
10049
10050 /* Product: '<S609>/Product2' */
10051 _rtB->table2D_Cn = _rtB->Sum_m[2] * _rtB->Sum_m[2];
10052
10053 /* Sum: '<S609>/Sum' */
10054 _rtB->u2rhoV2 = (_rtB->u2rhoV2 + _rtB->Product4_n) + _rtB->table2D_Cn;
10055
10056 /* MATLAB Function: '<S464>/Embedded MATLAB Function1' */
10057 /* MATLAB Function 'AFCS_MODEL1/Subsystem5/Subsystem3/Devond/Embedded MATLAB Function1': '<S604>:1' */
10058 /* %%%%%%% */
10059 /* '<S604>:1:3' */
10060 /* '<S604>:1:4' */
10061 TrigonometricFunction4_o = 6.371004E+6 * _rtB->Alt / (6.371004E+6 +
10062 _rtB->Alt);
10063
10064 /* %%%%%% */
10065 if (_rtB->Alt <= 11019.0) {
10066 /* '<S604>:1:6' */
10067 /* '<S604>:1:7' */
10068 T = 288.15 - 0.0065 * TrigonometricFunction4_o;
10069
10070 /* '<S604>:1:8' */
10071 _rtB->PS = AFCS_MODEL1_rt_powd_snf(1.0 - 2.2557700000000003E-5 *
10072 TrigonometricFunction4_o, 5.25588) * 101325.0;
10073
10074 /* '<S604>:1:9' */
10075 _rtB->rou = AFCS_MODEL1_rt_powd_snf(1.0 - 2.2557700000000003E-5 *
10076 TrigonometricFunction4_o, 4.25588) * 1.225;
10077 } else if (_rtB->Alt <= 20063.0) {
10078 /* '<S604>:1:10' */
10079 /* '<S604>:1:11' */
10080 T = 216.65;
10081
10082 /* '<S604>:1:12' */
10083 _rtB->PS = exp((TrigonometricFunction4_o - 11000.0) *
10084 -0.00015768850000000002) * 22632.04;
10085
10086 /* '<S604>:1:13' */
10087 _rtB->rou = exp((TrigonometricFunction4_o - 11000.0) *
10088 -0.00015768850000000002) * 0.3639176;
10089 } else if (_rtB->Alt <= 32162.0) {
10090 /* '<S604>:1:14' */
10091 /* '<S604>:1:15' */
10092 T = (TrigonometricFunction4_o - 20000.0) * 0.001 + 216.65;
10093
10094 /* '<S604>:1:16' */
10095 _rtB->PS = AFCS_MODEL1_rt_powd_snf((TrigonometricFunction4_o - 20000.0) *
10096 4.6157399999999991E-6 + 1.0, -34.16322) * 5474.879;
10097
10098 /* '<S604>:1:17' */
10099 _rtB->rou = AFCS_MODEL1_rt_powd_snf((TrigonometricFunction4_o - 20000.0)
10100 * 4.6157399999999991E-6 + 1.0, -35.16322) * 0.08803471;
10101 } else if (_rtB->Alt <= 47350.0) {
10102 /* '<S604>:1:18' */
10103 /* '<S604>:1:19' */
10104 T = (TrigonometricFunction4_o - 32000.0) * 0.0028 + 228.65;
10105
10106 /* '<S604>:1:20' */
10107 _rtB->PS = AFCS_MODEL1_rt_powd_snf((TrigonometricFunction4_o - 32000.0) *
10108 1.224579E-5 + 1.0, -12.20115) * 868.01600000000008;
10109
10110 /* '<S604>:1:21' */
10111 _rtB->rou = AFCS_MODEL1_rt_powd_snf((TrigonometricFunction4_o - 32000.0)
10112 * 1.224579E-5 + 1.0, -13.20115) * 0.01322497;
10113 } else if (_rtB->Alt <= 51413.0) {
10114 /* '<S604>:1:22' */
10115 /* '<S604>:1:23' */
10116 T = 270.65;
10117
10118 /* '<S604>:1:24' */
10119 _rtB->PS = exp((TrigonometricFunction4_o - 47000.0) *
10120 -0.00012622660000000002) * 110.90580000000001;
10121
10122 /* '<S604>:1:25' */
10123 _rtB->rou = exp((TrigonometricFunction4_o - 47000.0) *
10124 -0.00012622660000000002) * 0.001427527;
10125 } else if (_rtB->Alt < 71802.0) {
10126 /* '<S604>:1:26' */
10127 /* '<S604>:1:27' */
10128 T = 270.65 - (TrigonometricFunction4_o - 51000.0) * 0.0028;
10129
10130 /* '<S604>:1:28' */
10131 _rtB->PS = AFCS_MODEL1_rt_powd_snf(1.0 - (TrigonometricFunction4_o -
10132 51000.0) * 1.0345459999999998E-5, 12.20115) * 66.93853;
10133
10134 /* '<S604>:1:29' */
10135 _rtB->rou = AFCS_MODEL1_rt_powd_snf(1.0 - (TrigonometricFunction4_o -
10136 51000.0) * 1.0345459999999998E-5, 11.20115) * 0.00086160110000000007;
10137 } else if (_rtB->Alt < 86000.0) {
10138 /* '<S604>:1:30' */
10139 /* '<S604>:1:31' */
10140 T = 214.65 - (TrigonometricFunction4_o - 71000.0) * 0.002;
10141
10142 /* '<S604>:1:32' */
10143 _rtB->PS = AFCS_MODEL1_rt_powd_snf(1.0 - (TrigonometricFunction4_o -
10144 71000.0) * 9.3174939999999987E-6, 17.08161) * 3.956392;
10145
10146 /* '<S604>:1:33' */
10147 _rtB->rou = AFCS_MODEL1_rt_powd_snf(1.0 - (TrigonometricFunction4_o -
10148 71000.0) * 9.3174939999999987E-6, 16.08161) * 6.421057E-5;
10149 } else {
10150 /* '<S604>:1:35' */
10151 T = 186.87;
10152
10153 /* '<S604>:1:36' */
10154 _rtB->PS = (1.042E-6 * TrigonometricFunction4_o + 2.273) * 0.101325 *
10155 exp((87284.8 - TrigonometricFunction4_o) / 5470.0);
10156
10157 /* '<S604>:1:37' */
10158 _rtB->rou = exp((87284.8 - TrigonometricFunction4_o) / 5470.0) *
10159 4.4603475E-6;
10160 }
10161
10162 /* end */
10163 /* '<S604>:1:45' */
10164 _rtB->sound = 20.0468 * sqrt(T);
10165
10166 /* '<S604>:1:46' */
10167 TrigonometricFunction4_o = 6.356766E+6 / (6.356766E+6 + _rtB->Alt);
10168 _rtB->g = TrigonometricFunction4_o * TrigonometricFunction4_o * 9.80665;
10169
10170 /* End of MATLAB Function: '<S464>/Embedded MATLAB Function1' */
10171
10172 /* Product: '<S464>/Product' */
10173 _rtB->Product4_n = _rtB->rou;
10174
10175 /* Product: '<S602>/Product2' */
10176 _rtB->u2rhoV2 *= _rtB->Product4_n;
10177
10178 /* Gain: '<S602>/1//2rhoV^2' */
10179 _rtB->u2rhoV2 *= 0.5;
10180
10181 /* Product: '<S480>/Product4' incorporates:
10182 * Constant: '<S480>/Constant2'
10183 */
10184 _rtB->Product4_n = _rtB->u2rhoV2 * 1.952;
10185
10186 /* Selector: '<S876>/Selector1' */
10187 _rtB->Product2_f[0] = _rtB->Product4[3];
10188
10189 /* Gain: '<S876>/Gain1' */
10190 _rtB->VectorConcatenate_b[0] = _rtB->Product2_f[0];
10191
10192 /* Selector: '<S876>/Selector1' */
10193 _rtB->Product2_f[1] = _rtB->Product4[0];
10194
10195 /* Gain: '<S876>/Gain1' */
10196 _rtB->VectorConcatenate_b[1] = _rtB->Product2_f[1];
10197
10198 /* Selector: '<S876>/Selector1' */
10199 _rtB->Product2_f[2] = _rtB->Product4[6];
10200
10201 /* Gain: '<S876>/Gain1' */
10202 _rtB->VectorConcatenate_b[2] = -_rtB->Product2_f[2];
10203
10204 /* Selector: '<S876>/Selector2' */
10205 _rtB->sincos_o1_m[0] = _rtB->Product4[7];
10206
10207 /* Gain: '<S876>/Gain2' */
10208 _rtB->VectorConcatenate_b[3] = _rtB->sincos_o1_m[0];
10209
10210 /* Selector: '<S876>/Selector3' */
10211 _rtB->sincos_o1_m[0] = _rtB->Product4[1];
10212
10213 /* Gain: '<S876>/Gain3' */
10214 _rtB->VectorConcatenate_b[5] = -_rtB->sincos_o1_m[0];
10215
10216 /* Selector: '<S876>/Selector2' */
10217 _rtB->sincos_o1_m[1] = _rtB->Product4[8];
10218
10219 /* Gain: '<S876>/Gain2' */
10220 _rtB->VectorConcatenate_b[4] = _rtB->sincos_o1_m[1];
10221
10222 /* Selector: '<S876>/Selector3' */
10223 _rtB->sincos_o1_m[1] = _rtB->Product4[4];
10224
10225 /* Gain: '<S876>/Gain3' */
10226 _rtB->VectorConcatenate_b[6] = _rtB->sincos_o1_m[1];
10227
10228 /* If: '<S846>/If' */
10229 if (ssIsMajorTimeStep(S)) {
10230 rtAction = (int8_T)!((_rtB->VectorConcatenate_b[2] >= 1.0) ||
10231 (_rtB->VectorConcatenate_b[2] <= -1.0));
10232 ((int8_T *)ssGetDWork(S, 507))[0] = rtAction;
10233 } else {
10234 rtAction = ((int8_T *)ssGetDWork(S, 507))[0];
10235 }
10236
10237 switch (rtAction) {
10238 case 0:
10239 /* Outputs for IfAction SubSystem: '<S846>/AxisRotZeroR3' incorporates:
10240 * ActionPort: '<S875>/Action Port'
10241 */
10242 /* Fcn: '<S875>/Fcn1' */
10243 _rtB->Merge_n[0] = AFCS_MODEL1_rt_atan2d_snf(_rtB->VectorConcatenate_b[5],
10244 _rtB->VectorConcatenate_b[6]);
10245
10246 /* Fcn: '<S875>/Fcn2' */
10247 if (_rtB->VectorConcatenate_b[2] > 1.0) {
10248 TrigonometricFunction4_o = 1.0;
10249 } else if (_rtB->VectorConcatenate_b[2] < -1.0) {
10250 TrigonometricFunction4_o = -1.0;
10251 } else {
10252 TrigonometricFunction4_o = _rtB->VectorConcatenate_b[2];
10253 }
10254
10255 _rtB->Merge_n[1] = asin(TrigonometricFunction4_o);
10256
10257 /* End of Fcn: '<S875>/Fcn2' */
10258
10259 /* Fcn: '<S875>/Fcn3' */
10260 _rtB->Merge_n[2] = 0.0;
10261
10262 /* End of Outputs for SubSystem: '<S846>/AxisRotZeroR3' */
10263 break;
10264
10265 case 1:
10266 /* Outputs for IfAction SubSystem: '<S846>/AxisRotDefault' incorporates:
10267 * ActionPort: '<S874>/Action Port'
10268 */
10269 /* Fcn: '<S874>/Fcn1' */
10270 _rtB->Merge_n[0] = AFCS_MODEL1_rt_atan2d_snf(_rtB->VectorConcatenate_b[0],
10271 _rtB->VectorConcatenate_b[1]);
10272
10273 /* Fcn: '<S874>/Fcn2' */
10274 if (_rtB->VectorConcatenate_b[2] > 1.0) {
10275 TrigonometricFunction4_o = 1.0;
10276 } else if (_rtB->VectorConcatenate_b[2] < -1.0) {
10277 TrigonometricFunction4_o = -1.0;
10278 } else {
10279 TrigonometricFunction4_o = _rtB->VectorConcatenate_b[2];
10280 }
10281
10282 _rtB->Merge_n[1] = asin(TrigonometricFunction4_o);
10283
10284 /* End of Fcn: '<S874>/Fcn2' */
10285
10286 /* Fcn: '<S874>/Fcn3' */
10287 _rtB->Merge_n[2] = AFCS_MODEL1_rt_atan2d_snf(_rtB->VectorConcatenate_b[3],
10288 _rtB->VectorConcatenate_b[4]);
10289
10290 /* End of Outputs for SubSystem: '<S846>/AxisRotDefault' */
10291 break;
10292 }
10293
10294 /* End of If: '<S846>/If' */
10295
10296 /* Switch: '<S607>/Switch' */
10297 if (_rtB->UnitConversion_o[0] > 3.0) {
10298 /* Trigonometry: '<S605>/Incidence' */
10299 _rtB->Incidence = AFCS_MODEL1_rt_atan2d_snf(_rtB->Sum_m[2], _rtB->Sum_m
10300 [0]);
10301 _rtB->Switch_h = _rtB->Incidence;
10302 } else {
10303 _rtB->Switch_h = _rtB->Merge_n[1];
10304 }
10305
10306 /* End of Switch: '<S607>/Switch' */
10307
10308 /* Gain: '<S482>/Unit Conversion' */
10309 _rtB->table2D_Cn = 57.295779513082323 * _rtB->Switch_h;
10310
10311 /* Saturate: '<S484>/Saturation1' */
10312 if (_rtB->table2D_Cn > 20.0) {
10313 _rtB->table2D_Cn = 20.0;
10314 } else {
10315 if (_rtB->table2D_Cn < -20.0) {
10316 _rtB->table2D_Cn = -20.0;
10317 }
10318 }
10319
10320 /* End of Saturate: '<S484>/Saturation1' */
10321
10322 /* Gain: '<S588>/Unit Conversion' */
10323 _rtB->add_d = 0.017453292519943295 * _rtB->table2D_Cn;
10324
10325 /* Product: '<S612>/Product' */
10326 _rtB->UnitConversion_j = _rtB->Sum_m[0] * _rtB->Sum_m[0];
10327
10328 /* Product: '<S612>/Product1' */
10329 _rtB->saturation = _rtB->Sum_m[1] * _rtB->Sum_m[1];
10330
10331 /* Product: '<S612>/Product2' */
10332 _rtB->table2D_Cm_dr = _rtB->Sum_m[2] * _rtB->Sum_m[2];
10333
10334 /* Sum: '<S612>/Sum' */
10335 _rtB->UnitConversion_j = (_rtB->UnitConversion_j + _rtB->saturation) +
10336 _rtB->table2D_Cm_dr;
10337
10338 /* Sqrt: '<S605>/Airspeed' */
10339 _rtB->Airspeed = sqrt(_rtB->UnitConversion_j);
10340
10341 /* Switch: '<S607>/Switch1' incorporates:
10342 * Constant: '<S607>/Constant'
10343 */
10344 if (_rtB->UnitConversion_o[0] > 3.0) {
10345 /* Saturate: '<S605>/Saturation' */
10346 if (_rtB->Airspeed <= 0.001) {
10347 _rtB->Product_ei = 0.001;
10348 } else {
10349 _rtB->Product_ei = _rtB->Airspeed;
10350 }
10351
10352 /* End of Saturate: '<S605>/Saturation' */
10353
10354 /* Product: '<S605>/Product' */
10355 _rtB->Product_ei = _rtB->Sum_m[1] / _rtB->Product_ei;
10356
10357 /* Trigonometry: '<S605>/Sideslip' */
10358 if (_rtB->Product_ei > 1.0) {
10359 TrigonometricFunction4_o = 1.0;
10360 } else if (_rtB->Product_ei < -1.0) {
10361 TrigonometricFunction4_o = -1.0;
10362 } else {
10363 TrigonometricFunction4_o = _rtB->Product_ei;
10364 }
10365
10366 _rtB->Sideslip = asin(TrigonometricFunction4_o);
10367
10368 /* End of Trigonometry: '<S605>/Sideslip' */
10369 _rtB->UnitConversion_j = _rtB->Sideslip;
10370 } else {
10371 _rtB->UnitConversion_j = 0.0;
10372 }
10373
10374 /* End of Switch: '<S607>/Switch1' */
10375
10376 /* Gain: '<S481>/Unit Conversion' */
10377 _rtB->saturation = 57.295779513082323 * _rtB->UnitConversion_j;
10378
10379 /* Saturate: '<S484>/Saturation' */
10380 if (_rtB->saturation > 20.0) {
10381 _rtB->saturation = 20.0;
10382 } else {
10383 if (_rtB->saturation < -20.0) {
10384 _rtB->saturation = -20.0;
10385 }
10386 }
10387
10388 /* End of Saturate: '<S484>/Saturation' */
10389
10390 /* Gain: '<S589>/Unit Conversion' */
10391 _rtB->table2D_Cm_dr = 0.017453292519943295 * _rtB->saturation;
10392
10393 /* SignalConversion: '<S590>/TmpSignal ConversionAtsincosInport1' */
10394 _rtB->sincos_o1_m[0] = _rtB->add_d;
10395 _rtB->sincos_o1_m[1] = _rtB->table2D_Cm_dr;
10396
10397 /* Trigonometry: '<S590>/sincos' */
10398 TrigonometricFunction4_o = cos(_rtB->sincos_o1_m[0]);
10399 _rtB->sincos_o1_m[0] = sin(_rtB->sincos_o1_m[0]);
10400 _rtB->sincos_o2_h[0] = TrigonometricFunction4_o;
10401 TrigonometricFunction4_o = cos(_rtB->sincos_o1_m[1]);
10402 _rtB->sincos_o1_m[1] = sin(_rtB->sincos_o1_m[1]);
10403 _rtB->sincos_o2_h[1] = TrigonometricFunction4_o;
10404
10405 /* Product: '<S591>/u(3)*u(4)' */
10406 _rtB->VectorConcatenate_a[0] = _rtB->sincos_o2_h[0] * _rtB->sincos_o2_h[1];
10407
10408 /* Product: '<S594>/u(2)*u(3)' */
10409 _rtB->add_d = _rtB->sincos_o1_m[1] * _rtB->sincos_o2_h[0];
10410
10411 /* UnaryMinus: '<S594>/Unary Minus' */
10412 _rtB->VectorConcatenate_a[1] = -_rtB->add_d;
10413
10414 /* UnaryMinus: '<S597>/Unary Minus' */
10415 _rtB->VectorConcatenate_a[2] = -_rtB->sincos_o1_m[0];
10416
10417 /* SignalConversion: '<S600>/ConcatBufferAtVector ConcatenateIn4' */
10418 _rtB->VectorConcatenate_a[3] = _rtB->sincos_o1_m[1];
10419
10420 /* SignalConversion: '<S600>/ConcatBufferAtVector ConcatenateIn5' */
10421 _rtB->VectorConcatenate_a[4] = _rtB->sincos_o2_h[1];
10422
10423 /* SignalConversion: '<S600>/ConcatBufferAtVector ConcatenateIn6' incorporates:
10424 * Constant: '<S598>/Constant'
10425 */
10426 _rtB->VectorConcatenate_a[5] = 0.0;
10427
10428 /* Product: '<S593>/u(1)*u(4)' */
10429 _rtB->VectorConcatenate_a[6] = _rtB->sincos_o1_m[0] * _rtB->sincos_o2_h[1];
10430
10431 /* Product: '<S596>/u(1)*u(2)' */
10432 _rtB->add_d = _rtB->sincos_o1_m[0] * _rtB->sincos_o1_m[1];
10433
10434 /* UnaryMinus: '<S596>/Unary Minus' */
10435 _rtB->VectorConcatenate_a[7] = -_rtB->add_d;
10436
10437 /* SignalConversion: '<S600>/ConcatBufferAtVector ConcatenateIn9' */
10438 _rtB->VectorConcatenate_a[8] = _rtB->sincos_o2_h[0];
10439
10440 /* Math: '<S488>/Math Function' */
10441 for (i_1 = 0; i_1 < 3; i_1++) {
10442 _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_a[i_1];
10443 _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_a[i_1 + 3];
10444 _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_a[i_1 + 6];
10445 }
10446
10447 /* End of Math: '<S488>/Math Function' */
10448
10449 /* Gain: '<S487>/Gain17' */
10450 _rtB->table2D_Cn *= 0.017452006980802792;
10451
10452 /* Product: '<S497>/product' incorporates:
10453 * Constant: '<S497>/value'
10454 */
10455 _rtB->add_d = _rtB->table2D_Cn * 57.29;
10456
10457 /* Saturate: '<S497>/saturation' */
10458 if (_rtB->add_d > 30.0) {
10459 _rtB->add_d = 30.0;
10460 } else {
10461 if (_rtB->add_d < -6.0) {
10462 _rtB->add_d = -6.0;
10463 }
10464 }
10465
10466 /* End of Saturate: '<S497>/saturation' */
10467
10468 /* Lookup_n-D: '<S491>/table1D_CD_base' */
10469 _rtB->table2D_Cm_dr = AFCS_MODEL1_look1_binlxpw(_rtB->add_d,
10470 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled44, 18U);
10471
10472 /* Sum: '<S483>/Add1' */
10473 _rtB->table2D_Cn_dr -= _rtB->table2D_Cn_dar;
10474
10475 /* Gain: '<S483>/Gain1' */
10476 _rtB->table2D_Cn_dr *= 0.5;
10477
10478 /* Gain: '<S463>/Gain4' */
10479 _rtB->table2D_Cn_dar = -_rtB->table2D_Cn_dr;
10480
10481 /* Gain: '<S487>/Gain1' */
10482 _rtB->table2D_Cn_dar *= 0.017452006980802792;
10483
10484 /* Product: '<S499>/product' incorporates:
10485 * Constant: '<S499>/value'
10486 */
10487 _rtB->table2D_Cn_dar *= 57.29;
10488
10489 /* Saturate: '<S499>/saturation' */
10490 if (_rtB->table2D_Cn_dar > 30.0) {
10491 _rtB->table2D_Cn_dar = 30.0;
10492 } else {
10493 if (_rtB->table2D_Cn_dar < -30.0) {
10494 _rtB->table2D_Cn_dar = -30.0;
10495 }
10496 }
10497
10498 /* End of Saturate: '<S499>/saturation' */
10499
10500 /* Lookup_n-D: '<S491>/table2D_CD_dal' */
10501 _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dar,
10502 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10503 AFCS_MODEL1_ConstP.pooled46, AFCS_MODEL1_ConstP.pooled138, 19U);
10504
10505 /* Gain: '<S487>/Gain2' */
10506 _rtB->table2D_Cn_dr *= 0.017452006980802792;
10507
10508 /* Product: '<S500>/product' incorporates:
10509 * Constant: '<S500>/value'
10510 */
10511 _rtB->table2D_Cn_dr *= 57.29;
10512
10513 /* Saturate: '<S500>/saturation' */
10514 if (_rtB->table2D_Cn_dr > 30.0) {
10515 _rtB->table2D_Cn_dr = 30.0;
10516 } else {
10517 if (_rtB->table2D_Cn_dr < -30.0) {
10518 _rtB->table2D_Cn_dr = -30.0;
10519 }
10520 }
10521
10522 /* End of Saturate: '<S500>/saturation' */
10523
10524 /* Lookup_n-D: '<S491>/table2D_CD_dar' */
10525 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dr,
10526 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10527 AFCS_MODEL1_ConstP.pooled46, AFCS_MODEL1_ConstP.pooled138, 19U);
10528
10529 /* Lookup_n-D: '<S491>/table2D_CD_dfal' */
10530 _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)
10531 ->saturation, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10532 AFCS_MODEL1_ConstP.pooled48, AFCS_MODEL1_ConstP.pooled138, 19U);
10533
10534 /* Lookup_n-D: '<S491>/table2D_CD_dfar' */
10535 _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)
10536 ->saturation_c, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10537 AFCS_MODEL1_ConstP.pooled48, AFCS_MODEL1_ConstP.pooled138, 19U);
10538
10539 /* Sum: '<S483>/Add' */
10540 _rtB->table2D_Cm_dfar += _rtB->table2D_Cn_dfal;
10541
10542 /* Gain: '<S483>/Gain' */
10543 _rtB->table2D_Cm_dfar *= 0.5;
10544
10545 /* Gain: '<S487>/Gain3' */
10546 _rtB->table2D_Cm_dfar *= 0.017452006980802792;
10547
10548 /* Product: '<S501>/product' incorporates:
10549 * Constant: '<S501>/value'
10550 */
10551 _rtB->table2D_Cm_dfar *= 57.29;
10552
10553 /* Saturate: '<S501>/saturation' */
10554 if (_rtB->table2D_Cm_dfar > 30.0) {
10555 _rtB->table2D_Cm_dfar = 30.0;
10556 } else {
10557 if (_rtB->table2D_Cm_dfar < -30.0) {
10558 _rtB->table2D_Cm_dfar = -30.0;
10559 }
10560 }
10561
10562 /* End of Saturate: '<S501>/saturation' */
10563
10564 /* Lookup_n-D: '<S491>/table2D_CD_de' */
10565 _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->add_d,
10566 _rtB->table2D_Cm_dfar, AFCS_MODEL1_ConstP.pooled45,
10567 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled49,
10568 AFCS_MODEL1_ConstP.pooled138, 19U);
10569
10570 /* Gain: '<S487>/Gain4' */
10571 _rtB->add_e *= 0.017452006980802792;
10572
10573 /* Product: '<S504>/product' incorporates:
10574 * Constant: '<S504>/value'
10575 */
10576 _rtB->add_e *= 57.29;
10577
10578 /* Saturate: '<S504>/saturation' */
10579 if (_rtB->add_e > 30.0) {
10580 _rtB->add_e = 30.0;
10581 } else {
10582 if (_rtB->add_e < -30.0) {
10583 _rtB->add_e = -30.0;
10584 }
10585 }
10586
10587 /* End of Saturate: '<S504>/saturation' */
10588
10589 /* Lookup_n-D: '<S491>/table2D_CD_dr' */
10590 _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->add_e,
10591 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10592 AFCS_MODEL1_ConstP.pooled50, AFCS_MODEL1_ConstP.pooled138, 19U);
10593
10594 /* Sum: '<S491>/add' */
10595 _rtB->table2D_Cm_dr = (((((_rtB->table2D_Cm_dr + _rtB->add) + _rtB->add_b)
10596 + _rtB->jxi) + _rtB->jxi_k) + _rtB->table2D_Cn_dfal) + _rtB->ixk;
10597
10598 /* Lookup_n-D: '<S493>/table2D_CY_dal' */
10599 _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dar,
10600 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10601 AFCS_MODEL1_ConstP.pooled51, AFCS_MODEL1_ConstP.pooled138, 19U);
10602
10603 /* Lookup_n-D: '<S493>/table2D_CY_dar' */
10604 _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dr,
10605 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10606 AFCS_MODEL1_ConstP.pooled52, AFCS_MODEL1_ConstP.pooled138, 19U);
10607
10608 /* Lookup_n-D: '<S493>/table2D_CY_dfal' */
10609 _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)
10610 ->saturation, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10611 AFCS_MODEL1_ConstP.pooled53, AFCS_MODEL1_ConstP.pooled138, 19U);
10612
10613 /* Lookup_n-D: '<S493>/table2D_CY_dfar' */
10614 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)
10615 ->saturation_c, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10616 AFCS_MODEL1_ConstP.pooled54, AFCS_MODEL1_ConstP.pooled138, 19U);
10617
10618 /* Lookup_n-D: '<S493>/table2D_CY_dr' */
10619 _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->add_e,
10620 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10621 AFCS_MODEL1_ConstP.pooled55, AFCS_MODEL1_ConstP.pooled138, 19U);
10622
10623 /* Sum: '<S493>/add' */
10624 _rtB->ixk = (((_rtB->ixk + _rtB->jxi_k) + _rtB->jxi) + _rtB->add_b) +
10625 _rtB->add;
10626
10627 /* Lookup_n-D: '<S492>/table1D_CL_base' */
10628 _rtB->jxi_k = AFCS_MODEL1_look1_binlxpw(_rtB->add_d,
10629 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled56, 18U);
10630
10631 /* Lookup_n-D: '<S492>/table2D_CL_de' */
10632 _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cm_dfar,
10633 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10634 AFCS_MODEL1_ConstP.pooled57, AFCS_MODEL1_ConstP.pooled138, 19U);
10635
10636 /* Lookup_n-D: '<S492>/table2D_CL_dal' */
10637 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dar,
10638 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10639 AFCS_MODEL1_ConstP.pooled58, AFCS_MODEL1_ConstP.pooled138, 19U);
10640
10641 /* Lookup_n-D: '<S492>/table2D_CL_dar' */
10642 _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dr,
10643 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10644 AFCS_MODEL1_ConstP.pooled58, AFCS_MODEL1_ConstP.pooled138, 19U);
10645
10646 /* Lookup_n-D: '<S492>/table2D_CL_dfal' */
10647 _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->add_d,
10648 AFCS_MODEL1_rtC(S)->saturation, AFCS_MODEL1_ConstP.pooled45,
10649 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled59,
10650 AFCS_MODEL1_ConstP.pooled138, 19U);
10651
10652 /* Lookup_n-D: '<S492>/table2D_CL_dfar' */
10653 _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)
10654 ->saturation_c, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10655 AFCS_MODEL1_ConstP.pooled59, AFCS_MODEL1_ConstP.pooled138, 19U);
10656
10657 /* Lookup_n-D: '<S492>/table2D_CL_dr' */
10658 _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->add_e,
10659 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10660 AFCS_MODEL1_ConstP.pooled60, AFCS_MODEL1_ConstP.pooled138, 19U);
10661
10662 /* Sum: '<S492>/add' */
10663 _rtB->jxi_k = (((((_rtB->jxi_k + _rtB->jxi) + _rtB->add_b) + _rtB->add) +
10664 _rtB->table2D_Cn_dfal) + _rtB->jxi_p) + _rtB->ixk_a;
10665
10666 /* Lookup_n-D: '<S494>/table2D_Cl_dal' */
10667 _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dar,
10668 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10669 AFCS_MODEL1_ConstP.pooled61, AFCS_MODEL1_ConstP.pooled138, 19U);
10670
10671 /* Lookup_n-D: '<S494>/table2D_Cl_dar' */
10672 _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dr,
10673 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10674 AFCS_MODEL1_ConstP.pooled62, AFCS_MODEL1_ConstP.pooled138, 19U);
10675
10676 /* Lookup_n-D: '<S494>/table2D_Cl_dr' */
10677 _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->add_e,
10678 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10679 AFCS_MODEL1_ConstP.pooled63, AFCS_MODEL1_ConstP.pooled138, 19U);
10680
10681 /* Lookup_n-D: '<S494>/table2D_Cl_dfal' */
10682 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)
10683 ->saturation, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10684 AFCS_MODEL1_ConstP.pooled64, AFCS_MODEL1_ConstP.pooled138, 19U);
10685
10686 /* Lookup_n-D: '<S494>/table2D_Cl_dfar' */
10687 _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)
10688 ->saturation_c, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10689 AFCS_MODEL1_ConstP.pooled65, AFCS_MODEL1_ConstP.pooled138, 19U);
10690
10691 /* Sum: '<S494>/add' */
10692 _rtB->ixk_a = (((_rtB->ixk_a + _rtB->jxi_p) + _rtB->jxi) + _rtB->add_b) +
10693 _rtB->add;
10694
10695 /* Lookup_n-D: '<S495>/table1D_Cm_base' */
10696 _rtB->jxi_p = AFCS_MODEL1_look1_binlxpw(_rtB->add_d,
10697 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled66, 18U);
10698
10699 /* Lookup_n-D: '<S495>/table2D_Cm_de' */
10700 _rtB->table2D_Cm_dfar = AFCS_MODEL1_look2_binlxpw(_rtB->add_d,
10701 _rtB->table2D_Cm_dfar, AFCS_MODEL1_ConstP.pooled45,
10702 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled67,
10703 AFCS_MODEL1_ConstP.pooled138, 19U);
10704
10705 /* Lookup_n-D: '<S495>/table2D_Cm_dal' */
10706 _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dar,
10707 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10708 AFCS_MODEL1_ConstP.pooled68, AFCS_MODEL1_ConstP.pooled138, 19U);
10709
10710 /* Lookup_n-D: '<S495>/table2D_Cm_dar' */
10711 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->table2D_Cn_dr,
10712 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10713 AFCS_MODEL1_ConstP.pooled68, AFCS_MODEL1_ConstP.pooled138, 19U);
10714
10715 /* Lookup_n-D: '<S495>/table2D_Cm_dfal' */
10716 _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)
10717 ->saturation, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10718 AFCS_MODEL1_ConstP.pooled69, AFCS_MODEL1_ConstP.pooled138, 19U);
10719
10720 /* Lookup_n-D: '<S495>/table2D_Cm_dfar' */
10721 _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->add_d,
10722 AFCS_MODEL1_rtC(S)->saturation_c, AFCS_MODEL1_ConstP.pooled45,
10723 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled69,
10724 AFCS_MODEL1_ConstP.pooled138, 19U);
10725
10726 /* Lookup_n-D: '<S495>/table2D_Cm_dr' */
10727 _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->add_e,
10728 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10729 AFCS_MODEL1_ConstP.pooled70, AFCS_MODEL1_ConstP.pooled138, 19U);
10730
10731 /* Sum: '<S495>/add' */
10732 _rtB->jxi_p = (((((_rtB->jxi_p + _rtB->table2D_Cm_dfar) + _rtB->jxi) +
10733 _rtB->add_b) + _rtB->add) + _rtB->table2D_Cn_dfal) +
10734 _rtB->kxj;
10735
10736 /* Lookup_n-D: '<S496>/table2D_Cn_dal' */
10737 _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->add_d,
10738 _rtB->table2D_Cn_dar, AFCS_MODEL1_ConstP.pooled45,
10739 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled71,
10740 AFCS_MODEL1_ConstP.pooled138, 19U);
10741
10742 /* Lookup_n-D: '<S496>/table2D_Cn_dar' */
10743 _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->add_d,
10744 _rtB->table2D_Cn_dr, AFCS_MODEL1_ConstP.pooled45,
10745 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled72,
10746 AFCS_MODEL1_ConstP.pooled138, 19U);
10747
10748 /* Lookup_n-D: '<S496>/table2D_Cn_dr' */
10749 _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, _rtB->add_e,
10750 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10751 AFCS_MODEL1_ConstP.pooled73, AFCS_MODEL1_ConstP.pooled138, 19U);
10752
10753 /* Lookup_n-D: '<S496>/table2D_Cn_dfal' */
10754 _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)
10755 ->saturation, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10756 AFCS_MODEL1_ConstP.pooled74, AFCS_MODEL1_ConstP.pooled138, 19U);
10757
10758 /* Lookup_n-D: '<S496>/table2D_Cn_dfar' */
10759 _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->add_d, AFCS_MODEL1_rtC(S)
10760 ->saturation_c, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10761 AFCS_MODEL1_ConstP.pooled75, AFCS_MODEL1_ConstP.pooled138, 19U);
10762
10763 /* Sum: '<S496>/add' */
10764 _rtB->table2D_Cn_dar = (((_rtB->table2D_Cn_dar + _rtB->table2D_Cn_dr) +
10765 _rtB->add_e) + _rtB->kxj) + _rtB->add_d;
10766
10767 /* Product: '<S513>/product' incorporates:
10768 * Constant: '<S513>/value'
10769 */
10770 _rtB->kxj = _rtB->table2D_Cn * 57.29;
10771
10772 /* Saturate: '<S513>/saturation' */
10773 if (_rtB->kxj > 30.0) {
10774 _rtB->kxj = 30.0;
10775 } else {
10776 if (_rtB->kxj < -6.0) {
10777 _rtB->kxj = -6.0;
10778 }
10779 }
10780
10781 /* End of Saturate: '<S513>/saturation' */
10782
10783 /* Gain: '<S487>/Gain8' */
10784 _rtB->saturation *= 0.017452006980802792;
10785
10786 /* Product: '<S514>/product' incorporates:
10787 * Constant: '<S514>/value'
10788 */
10789 _rtB->jxi = _rtB->saturation * 57.29;
10790
10791 /* Saturate: '<S514>/saturation' */
10792 if (_rtB->jxi > 10.0) {
10793 _rtB->jxi = 10.0;
10794 } else {
10795 if (_rtB->jxi < -10.0) {
10796 _rtB->jxi = -10.0;
10797 }
10798 }
10799
10800 /* End of Saturate: '<S514>/saturation' */
10801
10802 /* Lookup_n-D: '<S507>/table2D_CD' */
10803 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, _rtB->jxi,
10804 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,
10805 AFCS_MODEL1_ConstP.table2D_CD_tableData, AFCS_MODEL1_ConstP.pooled139,
10806 19U);
10807
10808 /* Lookup_n-D: '<S509>/table2D_CY' */
10809 _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, _rtB->jxi,
10810 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,
10811 AFCS_MODEL1_ConstP.table2D_CY_tableData, AFCS_MODEL1_ConstP.pooled139,
10812 19U);
10813
10814 /* Lookup_n-D: '<S508>/table2D_CL' */
10815 _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, _rtB->jxi,
10816 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,
10817 AFCS_MODEL1_ConstP.table2D_CL_tableData, AFCS_MODEL1_ConstP.pooled139,
10818 19U);
10819
10820 /* Lookup_n-D: '<S510>/table2D_Cl' */
10821 _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, _rtB->jxi,
10822 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,
10823 AFCS_MODEL1_ConstP.table2D_Cl_tableData, AFCS_MODEL1_ConstP.pooled139,
10824 19U);
10825
10826 /* Lookup_n-D: '<S511>/table2D_Cm' */
10827 _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, _rtB->jxi,
10828 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,
10829 AFCS_MODEL1_ConstP.table2D_Cm_tableData, AFCS_MODEL1_ConstP.pooled139,
10830 19U);
10831
10832 /* Lookup_n-D: '<S512>/table2D_Cn' */
10833 _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, _rtB->jxi,
10834 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,
10835 AFCS_MODEL1_ConstP.table2D_Cn_tableData, AFCS_MODEL1_ConstP.pooled139,
10836 19U);
10837
10838 /* Sum: '<S489>/Sum' */
10839 _rtB->Sum2_g[0] = _rtB->table2D_Cm_dr + _rtB->add_b;
10840 _rtB->Sum2_g[1] = _rtB->ixk + _rtB->add;
10841 _rtB->Sum2_g[2] = _rtB->jxi_k + _rtB->add_d;
10842 _rtB->Sum2_g[3] = _rtB->ixk_a + _rtB->add_e;
10843 _rtB->Sum2_g[4] = _rtB->jxi_p + _rtB->table2D_Cn_dr;
10844 _rtB->Sum2_g[5] = _rtB->table2D_Cn_dar + _rtB->kxj;
10845
10846 /* SignalConversion: '<S490>/ConcatBufferAtMatrix Concatenate1In1' */
10847 for (i_1 = 0; i_1 < 6; i_1++) {
10848 _rtB->MatrixConcatenate1[i_1] = 0.0;
10849 }
10850
10851 /* End of SignalConversion: '<S490>/ConcatBufferAtMatrix Concatenate1In1' */
10852
10853 /* Product: '<S581>/product' incorporates:
10854 * Constant: '<S581>/value'
10855 */
10856 _rtB->kxj = _rtB->table2D_Cn * 57.29;
10857
10858 /* Saturate: '<S581>/saturation' */
10859 if (_rtB->kxj > 30.0) {
10860 _rtB->kxj = 30.0;
10861 } else {
10862 if (_rtB->kxj < -6.0) {
10863 _rtB->kxj = -6.0;
10864 }
10865 }
10866
10867 /* End of Saturate: '<S581>/saturation' */
10868
10869 /* Lookup_n-D: '<S575>/table1D_CD_base' */
10870 _rtB->ixk_a = AFCS_MODEL1_look1_binlxpw(_rtB->kxj,
10871 AFCS_MODEL1_ConstP.pooled45,
10872 AFCS_MODEL1_ConstP.table1D_CD_base_tableData, 18U);
10873
10874 /* Lookup_n-D: '<S575>/table2D_CD_dal' */
10875 _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10876 ->saturation_l, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10877 AFCS_MODEL1_ConstP.pooled78, AFCS_MODEL1_ConstP.pooled138, 19U);
10878
10879 /* Lookup_n-D: '<S575>/table2D_CD_dar' */
10880 _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10881 ->saturation_e, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10882 AFCS_MODEL1_ConstP.pooled78, AFCS_MODEL1_ConstP.pooled138, 19U);
10883
10884 /* Lookup_n-D: '<S575>/table2D_CD_de' */
10885 _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10886 ->saturation_k, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10887 AFCS_MODEL1_ConstP.table2D_CD_de_tableData, AFCS_MODEL1_ConstP.pooled138,
10888 19U);
10889
10890 /* Lookup_n-D: '<S575>/table2D_CD_dr' */
10891 _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10892 ->saturation_b, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10893 AFCS_MODEL1_ConstP.table2D_CD_dr_tableData, AFCS_MODEL1_ConstP.pooled138,
10894 19U);
10895
10896 /* Sum: '<S575>/add' */
10897 _rtB->ixk_a = (((_rtB->ixk_a + _rtB->jxi_p) + _rtB->ixk) + _rtB->jxi_k) +
10898 _rtB->jxi;
10899
10900 /* Lookup_n-D: '<S577>/table2D_CY_dal' */
10901 _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10902 ->saturation_l, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10903 AFCS_MODEL1_ConstP.table2D_CY_dal_tableData,
10904 AFCS_MODEL1_ConstP.pooled138, 19U);
10905
10906 /* Lookup_n-D: '<S577>/table2D_CY_dar' */
10907 _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10908 ->saturation_e, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10909 AFCS_MODEL1_ConstP.table2D_CY_dar_tableData,
10910 AFCS_MODEL1_ConstP.pooled138, 19U);
10911
10912 /* Lookup_n-D: '<S577>/table2D_CY_dr' */
10913 _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10914 ->saturation_b, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10915 AFCS_MODEL1_ConstP.table2D_CY_dr_tableData, AFCS_MODEL1_ConstP.pooled138,
10916 19U);
10917
10918 /* Sum: '<S577>/add' */
10919 _rtB->jxi_p = (_rtB->jxi_p + _rtB->ixk) + _rtB->jxi_k;
10920
10921 /* Lookup_n-D: '<S576>/table1D_CL_base' */
10922 _rtB->ixk = AFCS_MODEL1_look1_binlxpw(_rtB->kxj,
10923 AFCS_MODEL1_ConstP.pooled45,
10924 AFCS_MODEL1_ConstP.table1D_CL_base_tableData, 18U);
10925
10926 /* Lookup_n-D: '<S576>/table2D_CL_dal' */
10927 _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10928 ->saturation_l, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10929 AFCS_MODEL1_ConstP.pooled79, AFCS_MODEL1_ConstP.pooled138, 19U);
10930
10931 /* Lookup_n-D: '<S576>/table2D_CL_dar' */
10932 _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10933 ->saturation_e, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10934 AFCS_MODEL1_ConstP.pooled79, AFCS_MODEL1_ConstP.pooled138, 19U);
10935
10936 /* Lookup_n-D: '<S576>/table2D_CL_de' */
10937 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10938 ->saturation_k, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10939 AFCS_MODEL1_ConstP.table2D_CL_de_tableData, AFCS_MODEL1_ConstP.pooled138,
10940 19U);
10941
10942 /* Sum: '<S576>/add' */
10943 _rtB->ixk = ((_rtB->ixk + _rtB->jxi_k) + _rtB->jxi) + _rtB->add_b;
10944
10945 /* Lookup_n-D: '<S578>/table2D_Cl_dal' */
10946 _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10947 ->saturation_l, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10948 AFCS_MODEL1_ConstP.table2D_Cl_dal_tableData,
10949 AFCS_MODEL1_ConstP.pooled138, 19U);
10950
10951 /* Lookup_n-D: '<S578>/table2D_Cl_dar' */
10952 _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10953 ->saturation_e, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10954 AFCS_MODEL1_ConstP.table2D_Cl_dar_tableData,
10955 AFCS_MODEL1_ConstP.pooled138, 19U);
10956
10957 /* Lookup_n-D: '<S578>/table2D_Cl_dr' */
10958 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10959 ->saturation_b, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10960 AFCS_MODEL1_ConstP.table2D_Cl_dr_tableData, AFCS_MODEL1_ConstP.pooled138,
10961 19U);
10962
10963 /* Sum: '<S578>/add' */
10964 _rtB->jxi_k = (_rtB->jxi_k + _rtB->jxi) + _rtB->add_b;
10965
10966 /* Lookup_n-D: '<S579>/table1D_Cm_base' */
10967 _rtB->jxi = AFCS_MODEL1_look1_binlxpw(_rtB->kxj,
10968 AFCS_MODEL1_ConstP.pooled45,
10969 AFCS_MODEL1_ConstP.table1D_Cm_base_tableData, 18U);
10970
10971 /* Lookup_n-D: '<S579>/table2D_Cm_de' */
10972 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10973 ->saturation_k, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10974 AFCS_MODEL1_ConstP.table2D_Cm_de_tableData, AFCS_MODEL1_ConstP.pooled138,
10975 19U);
10976
10977 /* Lookup_n-D: '<S579>/table2D_Cm_dal' */
10978 _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10979 ->saturation_l, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10980 AFCS_MODEL1_ConstP.pooled80, AFCS_MODEL1_ConstP.pooled138, 19U);
10981
10982 /* Lookup_n-D: '<S579>/table2D_Cm_dar' */
10983 _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10984 ->saturation_e, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10985 AFCS_MODEL1_ConstP.pooled80, AFCS_MODEL1_ConstP.pooled138, 19U);
10986
10987 /* Lookup_n-D: '<S579>/table2D_Cm_dr' */
10988 _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10989 ->saturation_b, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
10990 AFCS_MODEL1_ConstP.table2D_Cm_dr_tableData, AFCS_MODEL1_ConstP.pooled138,
10991 19U);
10992
10993 /* Sum: '<S579>/add' */
10994 _rtB->jxi = (((_rtB->jxi + _rtB->add_b) + _rtB->add) + _rtB->add_d) +
10995 _rtB->add_e;
10996
10997 /* Lookup_n-D: '<S580>/table2D_Cn_dal' */
10998 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
10999 ->saturation_l, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11000 AFCS_MODEL1_ConstP.pooled81, AFCS_MODEL1_ConstP.pooled138, 19U);
11001
11002 /* Lookup_n-D: '<S580>/table2D_Cn_dar' */
11003 _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11004 ->saturation_e, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11005 AFCS_MODEL1_ConstP.pooled81, AFCS_MODEL1_ConstP.pooled138, 19U);
11006
11007 /* Lookup_n-D: '<S580>/table2D_Cn_dr' */
11008 _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11009 ->saturation_b, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11010 AFCS_MODEL1_ConstP.table2D_Cn_dr_tableData, AFCS_MODEL1_ConstP.pooled138,
11011 19U);
11012
11013 /* Sum: '<S580>/add' */
11014 _rtB->add_b = (_rtB->add_b + _rtB->add) + _rtB->kxj;
11015
11016 /* Product: '<S528>/product' incorporates:
11017 * Constant: '<S528>/value'
11018 */
11019 _rtB->kxj = _rtB->table2D_Cn * 57.29;
11020
11021 /* Saturate: '<S528>/saturation' */
11022 if (_rtB->kxj > 30.0) {
11023 _rtB->kxj = 30.0;
11024 } else {
11025 if (_rtB->kxj < -6.0) {
11026 _rtB->kxj = -6.0;
11027 }
11028 }
11029
11030 /* End of Saturate: '<S528>/saturation' */
11031
11032 /* Lookup_n-D: '<S522>/table1D_CD_base' */
11033 _rtB->add = AFCS_MODEL1_look1_binlxpw(_rtB->kxj,
11034 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled44, 18U);
11035
11036 /* Lookup_n-D: '<S522>/table2D_CD_dal' */
11037 _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11038 ->saturation_bo, AFCS_MODEL1_ConstP.pooled45,
11039 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled46,
11040 AFCS_MODEL1_ConstP.pooled138, 19U);
11041
11042 /* Lookup_n-D: '<S522>/table2D_CD_dar' */
11043 _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11044 ->saturation_n, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11045 AFCS_MODEL1_ConstP.pooled46, AFCS_MODEL1_ConstP.pooled138, 19U);
11046
11047 /* Lookup_n-D: '<S522>/table2D_CD_dfal' */
11048 _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11049 AFCS_MODEL1_rtC(S)->saturation_j, AFCS_MODEL1_ConstP.pooled45,
11050 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled48,
11051 AFCS_MODEL1_ConstP.pooled138, 19U);
11052
11053 /* Lookup_n-D: '<S522>/table2D_CD_dfar' */
11054 _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC
11055 (S)->saturation_l3, AFCS_MODEL1_ConstP.pooled45,
11056 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled48,
11057 AFCS_MODEL1_ConstP.pooled138, 19U);
11058
11059 /* Lookup_n-D: '<S522>/table2D_CD_de' */
11060 _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11061 AFCS_MODEL1_rtC(S)->saturation_k0, AFCS_MODEL1_ConstP.pooled45,
11062 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled49,
11063 AFCS_MODEL1_ConstP.pooled138, 19U);
11064
11065 /* Lookup_n-D: '<S522>/table2D_CD_dr' */
11066 _rtB->table2D_Cm_dfar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11067 AFCS_MODEL1_rtC(S)->saturation_h, AFCS_MODEL1_ConstP.pooled45,
11068 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled50,
11069 AFCS_MODEL1_ConstP.pooled138, 19U);
11070
11071 /* Sum: '<S522>/add' */
11072 _rtB->add = (((((_rtB->add + _rtB->add_d) + _rtB->add_e) +
11073 _rtB->table2D_Cn_dar) + _rtB->table2D_Cn_dr) +
11074 _rtB->table2D_Cn_dfal) + _rtB->table2D_Cm_dfar;
11075
11076 /* Lookup_n-D: '<S524>/table2D_CY_dal' */
11077 _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11078 ->saturation_bo, AFCS_MODEL1_ConstP.pooled45,
11079 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled51,
11080 AFCS_MODEL1_ConstP.pooled138, 19U);
11081
11082 /* Lookup_n-D: '<S524>/table2D_CY_dar' */
11083 _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11084 ->saturation_n, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11085 AFCS_MODEL1_ConstP.pooled52, AFCS_MODEL1_ConstP.pooled138, 19U);
11086
11087 /* Lookup_n-D: '<S524>/table2D_CY_dfal' */
11088 _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11089 AFCS_MODEL1_rtC(S)->saturation_j, AFCS_MODEL1_ConstP.pooled45,
11090 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled53,
11091 AFCS_MODEL1_ConstP.pooled138, 19U);
11092
11093 /* Lookup_n-D: '<S524>/table2D_CY_dfar' */
11094 _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC
11095 (S)->saturation_l3, AFCS_MODEL1_ConstP.pooled45,
11096 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled54,
11097 AFCS_MODEL1_ConstP.pooled138, 19U);
11098
11099 /* Lookup_n-D: '<S524>/table2D_CY_dr' */
11100 _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11101 AFCS_MODEL1_rtC(S)->saturation_h, AFCS_MODEL1_ConstP.pooled45,
11102 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled55,
11103 AFCS_MODEL1_ConstP.pooled138, 19U);
11104
11105 /* Sum: '<S524>/add' */
11106 _rtB->add_d = (((_rtB->add_d + _rtB->add_e) + _rtB->table2D_Cn_dar) +
11107 _rtB->table2D_Cn_dr) + _rtB->table2D_Cn_dfal;
11108
11109 /* Lookup_n-D: '<S523>/table1D_CL_base' */
11110 _rtB->add_e = AFCS_MODEL1_look1_binlxpw(_rtB->kxj,
11111 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled56, 18U);
11112
11113 /* Lookup_n-D: '<S523>/table2D_CL_de' */
11114 _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11115 AFCS_MODEL1_rtC(S)->saturation_k0, AFCS_MODEL1_ConstP.pooled45,
11116 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled57,
11117 AFCS_MODEL1_ConstP.pooled138, 19U);
11118
11119 /* Lookup_n-D: '<S523>/table2D_CL_dal' */
11120 _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC
11121 (S)->saturation_bo, AFCS_MODEL1_ConstP.pooled45,
11122 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled58,
11123 AFCS_MODEL1_ConstP.pooled138, 19U);
11124
11125 /* Lookup_n-D: '<S523>/table2D_CL_dar' */
11126 _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11127 AFCS_MODEL1_rtC(S)->saturation_n, AFCS_MODEL1_ConstP.pooled45,
11128 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled58,
11129 AFCS_MODEL1_ConstP.pooled138, 19U);
11130
11131 /* Lookup_n-D: '<S523>/table2D_CL_dfal' */
11132 _rtB->table2D_Cm_dfar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11133 AFCS_MODEL1_rtC(S)->saturation_j, AFCS_MODEL1_ConstP.pooled45,
11134 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled59,
11135 AFCS_MODEL1_ConstP.pooled138, 19U);
11136
11137 /* Lookup_n-D: '<S523>/table2D_CL_dfar' */
11138 _rtB->table2D_Cm_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC
11139 (S)->saturation_l3, AFCS_MODEL1_ConstP.pooled45,
11140 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled59,
11141 AFCS_MODEL1_ConstP.pooled138, 19U);
11142
11143 /* Lookup_n-D: '<S523>/table2D_CL_dr' */
11144 _rtB->ixj = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11145 ->saturation_h, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11146 AFCS_MODEL1_ConstP.pooled60, AFCS_MODEL1_ConstP.pooled138, 19U);
11147
11148 /* Sum: '<S523>/add' */
11149 _rtB->add_e = (((((_rtB->add_e + _rtB->table2D_Cn_dar) +
11150 _rtB->table2D_Cn_dr) + _rtB->table2D_Cn_dfal) +
11151 _rtB->table2D_Cm_dfar) + _rtB->table2D_Cm_dr) + _rtB->ixj;
11152
11153 /* Lookup_n-D: '<S525>/table2D_Cl_dal' */
11154 _rtB->ixj = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11155 ->saturation_bo, AFCS_MODEL1_ConstP.pooled45,
11156 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled61,
11157 AFCS_MODEL1_ConstP.pooled138, 19U);
11158
11159 /* Lookup_n-D: '<S525>/table2D_Cl_dar' */
11160 _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11161 AFCS_MODEL1_rtC(S)->saturation_n, AFCS_MODEL1_ConstP.pooled45,
11162 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled62,
11163 AFCS_MODEL1_ConstP.pooled138, 19U);
11164
11165 /* Lookup_n-D: '<S525>/table2D_Cl_dr' */
11166 _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC
11167 (S)->saturation_h, AFCS_MODEL1_ConstP.pooled45,
11168 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled63,
11169 AFCS_MODEL1_ConstP.pooled138, 19U);
11170
11171 /* Lookup_n-D: '<S525>/table2D_Cl_dfal' */
11172 _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11173 AFCS_MODEL1_rtC(S)->saturation_j, AFCS_MODEL1_ConstP.pooled45,
11174 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled64,
11175 AFCS_MODEL1_ConstP.pooled138, 19U);
11176
11177 /* Lookup_n-D: '<S525>/table2D_Cl_dfar' */
11178 _rtB->table2D_Cm_dfar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11179 AFCS_MODEL1_rtC(S)->saturation_l3, AFCS_MODEL1_ConstP.pooled45,
11180 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled65,
11181 AFCS_MODEL1_ConstP.pooled138, 19U);
11182
11183 /* Sum: '<S525>/add' */
11184 _rtB->ixj = (((_rtB->ixj + _rtB->table2D_Cn_dar) + _rtB->table2D_Cn_dr) +
11185 _rtB->table2D_Cn_dfal) + _rtB->table2D_Cm_dfar;
11186
11187 /* Lookup_n-D: '<S526>/table1D_Cm_base' */
11188 _rtB->table2D_Cn_dar = AFCS_MODEL1_look1_binlxpw(_rtB->kxj,
11189 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled66, 18U);
11190
11191 /* Lookup_n-D: '<S526>/table2D_Cm_de' */
11192 _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC
11193 (S)->saturation_k0, AFCS_MODEL1_ConstP.pooled45,
11194 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled67,
11195 AFCS_MODEL1_ConstP.pooled138, 19U);
11196
11197 /* Lookup_n-D: '<S526>/table2D_Cm_dal' */
11198 _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11199 AFCS_MODEL1_rtC(S)->saturation_bo, AFCS_MODEL1_ConstP.pooled45,
11200 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled68,
11201 AFCS_MODEL1_ConstP.pooled138, 19U);
11202
11203 /* Lookup_n-D: '<S526>/table2D_Cm_dar' */
11204 _rtB->table2D_Cm_dfar = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11205 AFCS_MODEL1_rtC(S)->saturation_n, AFCS_MODEL1_ConstP.pooled45,
11206 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled68,
11207 AFCS_MODEL1_ConstP.pooled138, 19U);
11208
11209 /* Lookup_n-D: '<S526>/table2D_Cm_dfal' */
11210 _rtB->table2D_Cm_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC
11211 (S)->saturation_j, AFCS_MODEL1_ConstP.pooled45,
11212 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled69,
11213 AFCS_MODEL1_ConstP.pooled138, 19U);
11214
11215 /* Lookup_n-D: '<S526>/table2D_Cm_dfar' */
11216 _rtB->kxi = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11217 ->saturation_l3, AFCS_MODEL1_ConstP.pooled45,
11218 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled69,
11219 AFCS_MODEL1_ConstP.pooled138, 19U);
11220
11221 /* Lookup_n-D: '<S526>/table2D_Cm_dr' */
11222 _rtB->jxk = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11223 ->saturation_h, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11224 AFCS_MODEL1_ConstP.pooled70, AFCS_MODEL1_ConstP.pooled138, 19U);
11225
11226 /* Sum: '<S526>/add' */
11227 _rtB->table2D_Cn_dar = (((((_rtB->table2D_Cn_dar + _rtB->table2D_Cn_dr) +
11228 _rtB->table2D_Cn_dfal) + _rtB->table2D_Cm_dfar) + _rtB->table2D_Cm_dr) +
11229 _rtB->kxi) + _rtB->jxk;
11230
11231 /* Lookup_n-D: '<S527>/table2D_Cn_dal' */
11232 _rtB->jxk = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11233 ->saturation_bo, AFCS_MODEL1_ConstP.pooled45,
11234 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled71,
11235 AFCS_MODEL1_ConstP.pooled138, 19U);
11236
11237 /* Lookup_n-D: '<S527>/table2D_Cn_dar' */
11238 _rtB->kxi = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11239 ->saturation_n, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11240 AFCS_MODEL1_ConstP.pooled72, AFCS_MODEL1_ConstP.pooled138, 19U);
11241
11242 /* Lookup_n-D: '<S527>/table2D_Cn_dr' */
11243 _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC
11244 (S)->saturation_h, AFCS_MODEL1_ConstP.pooled45,
11245 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled73,
11246 AFCS_MODEL1_ConstP.pooled138, 19U);
11247
11248 /* Lookup_n-D: '<S527>/table2D_Cn_dfal' */
11249 _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->kxj,
11250 AFCS_MODEL1_rtC(S)->saturation_j, AFCS_MODEL1_ConstP.pooled45,
11251 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled74,
11252 AFCS_MODEL1_ConstP.pooled138, 19U);
11253
11254 /* Lookup_n-D: '<S527>/table2D_Cn_dfar' */
11255 _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->kxj, AFCS_MODEL1_rtC(S)
11256 ->saturation_l3, AFCS_MODEL1_ConstP.pooled45,
11257 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled75,
11258 AFCS_MODEL1_ConstP.pooled138, 19U);
11259
11260 /* Sum: '<S527>/add' */
11261 _rtB->jxk = (((_rtB->jxk + _rtB->kxi) + _rtB->table2D_Cn_dr) +
11262 _rtB->table2D_Cn_dfal) + _rtB->kxj;
11263
11264 /* Sum: '<S490>/Subtract' */
11265 _rtB->MatrixConcatenate1[6] = _rtB->ixk_a - _rtB->add;
11266 _rtB->MatrixConcatenate1[7] = _rtB->jxi_p - _rtB->add_d;
11267 _rtB->MatrixConcatenate1[8] = _rtB->ixk - _rtB->add_e;
11268 _rtB->MatrixConcatenate1[9] = _rtB->jxi_k - _rtB->ixj;
11269 _rtB->MatrixConcatenate1[10] = _rtB->jxi - _rtB->table2D_Cn_dar;
11270 _rtB->MatrixConcatenate1[11] = _rtB->add_b - _rtB->jxk;
11271
11272 /* Product: '<S558>/product' incorporates:
11273 * Constant: '<S558>/value'
11274 */
11275 _rtB->jxk = _rtB->table2D_Cn * 57.29;
11276
11277 /* Saturate: '<S558>/saturation' */
11278 if (_rtB->jxk > 30.0) {
11279 _rtB->jxk = 30.0;
11280 } else {
11281 if (_rtB->jxk < -6.0) {
11282 _rtB->jxk = -6.0;
11283 }
11284 }
11285
11286 /* End of Saturate: '<S558>/saturation' */
11287
11288 /* Lookup_n-D: '<S552>/table1D_CD_base' */
11289 _rtB->kxi = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,
11290 AFCS_MODEL1_ConstP.pooled45,
11291 AFCS_MODEL1_ConstP.table1D_CD_base_tableData_a, 18U);
11292
11293 /* Lookup_n-D: '<S552>/table2D_CD_dal' */
11294 _rtB->ixj = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11295 ->saturation_jp, AFCS_MODEL1_ConstP.pooled45,
11296 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled83,
11297 AFCS_MODEL1_ConstP.pooled138, 19U);
11298
11299 /* Lookup_n-D: '<S552>/table2D_CD_dar' */
11300 _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11301 ->saturation_m, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11302 AFCS_MODEL1_ConstP.pooled83, AFCS_MODEL1_ConstP.pooled138, 19U);
11303
11304 /* Lookup_n-D: '<S552>/table2D_CD_de' */
11305 _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11306 ->saturation_d, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11307 AFCS_MODEL1_ConstP.table2D_CD_de_tableData_b,
11308 AFCS_MODEL1_ConstP.pooled138, 19U);
11309
11310 /* Lookup_n-D: '<S552>/table2D_CD_dr' */
11311 _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11312 ->saturation_nm, AFCS_MODEL1_ConstP.pooled45,
11313 AFCS_MODEL1_ConstP.pooled47,
11314 AFCS_MODEL1_ConstP.table2D_CD_dr_tableData_o,
11315 AFCS_MODEL1_ConstP.pooled138, 19U);
11316
11317 /* Sum: '<S552>/add' */
11318 _rtB->kxi = (((_rtB->kxi + _rtB->ixj) + _rtB->kxj) + _rtB->ixk_a) +
11319 _rtB->jxi_p;
11320
11321 /* Lookup_n-D: '<S554>/table2D_CY_dal' */
11322 _rtB->ixj = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11323 ->saturation_jp, AFCS_MODEL1_ConstP.pooled45,
11324 AFCS_MODEL1_ConstP.pooled47,
11325 AFCS_MODEL1_ConstP.table2D_CY_dal_tableData_e,
11326 AFCS_MODEL1_ConstP.pooled138, 19U);
11327
11328 /* Lookup_n-D: '<S554>/table2D_CY_dar' */
11329 _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11330 ->saturation_m, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11331 AFCS_MODEL1_ConstP.table2D_CY_dar_tableData_l,
11332 AFCS_MODEL1_ConstP.pooled138, 19U);
11333
11334 /* Lookup_n-D: '<S554>/table2D_CY_dr' */
11335 _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11336 ->saturation_nm, AFCS_MODEL1_ConstP.pooled45,
11337 AFCS_MODEL1_ConstP.pooled47,
11338 AFCS_MODEL1_ConstP.table2D_CY_dr_tableData_g,
11339 AFCS_MODEL1_ConstP.pooled138, 19U);
11340
11341 /* Sum: '<S554>/add' */
11342 _rtB->ixj = (_rtB->ixj + _rtB->kxj) + _rtB->ixk_a;
11343
11344 /* Lookup_n-D: '<S553>/table1D_CL_base' */
11345 _rtB->kxj = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,
11346 AFCS_MODEL1_ConstP.pooled45,
11347 AFCS_MODEL1_ConstP.table1D_CL_base_tableData_l, 18U);
11348
11349 /* Lookup_n-D: '<S553>/table2D_CL_dal' */
11350 _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11351 ->saturation_jp, AFCS_MODEL1_ConstP.pooled45,
11352 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled84,
11353 AFCS_MODEL1_ConstP.pooled138, 19U);
11354
11355 /* Lookup_n-D: '<S553>/table2D_CL_dar' */
11356 _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11357 ->saturation_m, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11358 AFCS_MODEL1_ConstP.pooled84, AFCS_MODEL1_ConstP.pooled138, 19U);
11359
11360 /* Lookup_n-D: '<S553>/table2D_CL_de' */
11361 _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11362 ->saturation_d, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11363 AFCS_MODEL1_ConstP.table2D_CL_de_tableData_d,
11364 AFCS_MODEL1_ConstP.pooled138, 19U);
11365
11366 /* Sum: '<S553>/add' */
11367 _rtB->kxj = ((_rtB->kxj + _rtB->ixk_a) + _rtB->jxi_p) + _rtB->ixk;
11368
11369 /* Lookup_n-D: '<S555>/table2D_Cl_dal' */
11370 _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11371 ->saturation_jp, AFCS_MODEL1_ConstP.pooled45,
11372 AFCS_MODEL1_ConstP.pooled47,
11373 AFCS_MODEL1_ConstP.table2D_Cl_dal_tableData_n,
11374 AFCS_MODEL1_ConstP.pooled138, 19U);
11375
11376 /* Lookup_n-D: '<S555>/table2D_Cl_dar' */
11377 _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11378 ->saturation_m, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11379 AFCS_MODEL1_ConstP.table2D_Cl_dar_tableData_k,
11380 AFCS_MODEL1_ConstP.pooled138, 19U);
11381
11382 /* Lookup_n-D: '<S555>/table2D_Cl_dr' */
11383 _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11384 ->saturation_nm, AFCS_MODEL1_ConstP.pooled45,
11385 AFCS_MODEL1_ConstP.pooled47,
11386 AFCS_MODEL1_ConstP.table2D_Cl_dr_tableData_h,
11387 AFCS_MODEL1_ConstP.pooled138, 19U);
11388
11389 /* Sum: '<S555>/add' */
11390 _rtB->ixk_a = (_rtB->ixk_a + _rtB->jxi_p) + _rtB->ixk;
11391
11392 /* Lookup_n-D: '<S556>/table1D_Cm_base' */
11393 _rtB->jxi_p = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,
11394 AFCS_MODEL1_ConstP.pooled45,
11395 AFCS_MODEL1_ConstP.table1D_Cm_base_tableData_l, 18U);
11396
11397 /* Lookup_n-D: '<S556>/table2D_Cm_de' */
11398 _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11399 ->saturation_d, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11400 AFCS_MODEL1_ConstP.table2D_Cm_de_tableData_h,
11401 AFCS_MODEL1_ConstP.pooled138, 19U);
11402
11403 /* Lookup_n-D: '<S556>/table2D_Cm_dal' */
11404 _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11405 ->saturation_jp, AFCS_MODEL1_ConstP.pooled45,
11406 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled85,
11407 AFCS_MODEL1_ConstP.pooled138, 19U);
11408
11409 /* Lookup_n-D: '<S556>/table2D_Cm_dar' */
11410 _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11411 ->saturation_m, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11412 AFCS_MODEL1_ConstP.pooled85, AFCS_MODEL1_ConstP.pooled138, 19U);
11413
11414 /* Lookup_n-D: '<S556>/table2D_Cm_dr' */
11415 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11416 ->saturation_nm, AFCS_MODEL1_ConstP.pooled45,
11417 AFCS_MODEL1_ConstP.pooled47,
11418 AFCS_MODEL1_ConstP.table2D_Cm_dr_tableData_f,
11419 AFCS_MODEL1_ConstP.pooled138, 19U);
11420
11421 /* Sum: '<S556>/add' */
11422 _rtB->jxi_p = (((_rtB->jxi_p + _rtB->ixk) + _rtB->jxi_k) + _rtB->jxi) +
11423 _rtB->add_b;
11424
11425 /* Lookup_n-D: '<S557>/table2D_Cn_dal' */
11426 _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11427 ->saturation_jp, AFCS_MODEL1_ConstP.pooled45,
11428 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.table2D_Cn_dal_tableData,
11429 AFCS_MODEL1_ConstP.pooled138, 19U);
11430
11431 /* Lookup_n-D: '<S557>/table2D_Cn_dar' */
11432 _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11433 ->saturation_m, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11434 AFCS_MODEL1_ConstP.table2D_Cn_dar_tableData,
11435 AFCS_MODEL1_ConstP.pooled138, 19U);
11436
11437 /* Lookup_n-D: '<S557>/table2D_Cn_dr' */
11438 _rtB->jxk = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11439 ->saturation_nm, AFCS_MODEL1_ConstP.pooled45,
11440 AFCS_MODEL1_ConstP.pooled47,
11441 AFCS_MODEL1_ConstP.table2D_Cn_dr_tableData_a,
11442 AFCS_MODEL1_ConstP.pooled138, 19U);
11443
11444 /* Sum: '<S557>/add' */
11445 _rtB->ixk = (_rtB->ixk + _rtB->jxi_k) + _rtB->jxk;
11446
11447 /* Product: '<S543>/product' incorporates:
11448 * Constant: '<S543>/value'
11449 */
11450 _rtB->jxk = _rtB->table2D_Cn * 57.29;
11451
11452 /* Saturate: '<S543>/saturation' */
11453 if (_rtB->jxk > 30.0) {
11454 _rtB->jxk = 30.0;
11455 } else {
11456 if (_rtB->jxk < -6.0) {
11457 _rtB->jxk = -6.0;
11458 }
11459 }
11460
11461 /* End of Saturate: '<S543>/saturation' */
11462
11463 /* Lookup_n-D: '<S537>/table1D_CD_base' */
11464 _rtB->jxi_k = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,
11465 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled44, 18U);
11466
11467 /* Lookup_n-D: '<S537>/table2D_CD_dal' */
11468 _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11469 ->saturation_g, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11470 AFCS_MODEL1_ConstP.pooled46, AFCS_MODEL1_ConstP.pooled138, 19U);
11471
11472 /* Lookup_n-D: '<S537>/table2D_CD_dar' */
11473 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11474 ->saturation_gs, AFCS_MODEL1_ConstP.pooled45,
11475 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled46,
11476 AFCS_MODEL1_ConstP.pooled138, 19U);
11477
11478 /* Lookup_n-D: '<S537>/table2D_CD_dfal' */
11479 _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11480 ->saturation_k0r, AFCS_MODEL1_ConstP.pooled45,
11481 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled48,
11482 AFCS_MODEL1_ConstP.pooled138, 19U);
11483
11484 /* Lookup_n-D: '<S537>/table2D_CD_dfar' */
11485 _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11486 ->saturation_f, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11487 AFCS_MODEL1_ConstP.pooled48, AFCS_MODEL1_ConstP.pooled138, 19U);
11488
11489 /* Lookup_n-D: '<S537>/table2D_CD_de' */
11490 _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11491 ->saturation_ng, AFCS_MODEL1_ConstP.pooled45,
11492 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled49,
11493 AFCS_MODEL1_ConstP.pooled138, 19U);
11494
11495 /* Lookup_n-D: '<S537>/table2D_CD_dr' */
11496 _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,
11497 AFCS_MODEL1_rtC(S)->saturation_by, AFCS_MODEL1_ConstP.pooled45,
11498 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled50,
11499 AFCS_MODEL1_ConstP.pooled138, 19U);
11500
11501 /* Sum: '<S537>/add' */
11502 _rtB->jxi_k = (((((_rtB->jxi_k + _rtB->jxi) + _rtB->add_b) + _rtB->add) +
11503 _rtB->add_d) + _rtB->add_e) + _rtB->table2D_Cn_dar;
11504
11505 /* Lookup_n-D: '<S539>/table2D_CY_dal' */
11506 _rtB->jxi = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11507 ->saturation_g, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11508 AFCS_MODEL1_ConstP.pooled51, AFCS_MODEL1_ConstP.pooled138, 19U);
11509
11510 /* Lookup_n-D: '<S539>/table2D_CY_dar' */
11511 _rtB->add_b = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11512 ->saturation_gs, AFCS_MODEL1_ConstP.pooled45,
11513 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled52,
11514 AFCS_MODEL1_ConstP.pooled138, 19U);
11515
11516 /* Lookup_n-D: '<S539>/table2D_CY_dfal' */
11517 _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11518 ->saturation_k0r, AFCS_MODEL1_ConstP.pooled45,
11519 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled53,
11520 AFCS_MODEL1_ConstP.pooled138, 19U);
11521
11522 /* Lookup_n-D: '<S539>/table2D_CY_dfar' */
11523 _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11524 ->saturation_f, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11525 AFCS_MODEL1_ConstP.pooled54, AFCS_MODEL1_ConstP.pooled138, 19U);
11526
11527 /* Lookup_n-D: '<S539>/table2D_CY_dr' */
11528 _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11529 ->saturation_by, AFCS_MODEL1_ConstP.pooled45,
11530 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled55,
11531 AFCS_MODEL1_ConstP.pooled138, 19U);
11532
11533 /* Sum: '<S539>/add' */
11534 _rtB->jxi = (((_rtB->jxi + _rtB->add_b) + _rtB->add) + _rtB->add_d) +
11535 _rtB->add_e;
11536
11537 /* Lookup_n-D: '<S538>/table1D_CL_base' */
11538 _rtB->add_b = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,
11539 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled56, 18U);
11540
11541 /* Lookup_n-D: '<S538>/table2D_CL_de' */
11542 _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11543 ->saturation_ng, AFCS_MODEL1_ConstP.pooled45,
11544 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled57,
11545 AFCS_MODEL1_ConstP.pooled138, 19U);
11546
11547 /* Lookup_n-D: '<S538>/table2D_CL_dal' */
11548 _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11549 ->saturation_g, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11550 AFCS_MODEL1_ConstP.pooled58, AFCS_MODEL1_ConstP.pooled138, 19U);
11551
11552 /* Lookup_n-D: '<S538>/table2D_CL_dar' */
11553 _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11554 ->saturation_gs, AFCS_MODEL1_ConstP.pooled45,
11555 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled58,
11556 AFCS_MODEL1_ConstP.pooled138, 19U);
11557
11558 /* Lookup_n-D: '<S538>/table2D_CL_dfal' */
11559 _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,
11560 AFCS_MODEL1_rtC(S)->saturation_k0r, AFCS_MODEL1_ConstP.pooled45,
11561 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled59,
11562 AFCS_MODEL1_ConstP.pooled138, 19U);
11563
11564 /* Lookup_n-D: '<S538>/table2D_CL_dfar' */
11565 _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC
11566 (S)->saturation_f, AFCS_MODEL1_ConstP.pooled45,
11567 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled59,
11568 AFCS_MODEL1_ConstP.pooled138, 19U);
11569
11570 /* Lookup_n-D: '<S538>/table2D_CL_dr' */
11571 _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,
11572 AFCS_MODEL1_rtC(S)->saturation_by, AFCS_MODEL1_ConstP.pooled45,
11573 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled60,
11574 AFCS_MODEL1_ConstP.pooled138, 19U);
11575
11576 /* Sum: '<S538>/add' */
11577 _rtB->add_b = (((((_rtB->add_b + _rtB->add) + _rtB->add_d) + _rtB->add_e)
11578 + _rtB->table2D_Cn_dar) + _rtB->table2D_Cn_dr) +
11579 _rtB->table2D_Cn_dfal;
11580
11581 /* Lookup_n-D: '<S540>/table2D_Cl_dal' */
11582 _rtB->add = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11583 ->saturation_g, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11584 AFCS_MODEL1_ConstP.pooled61, AFCS_MODEL1_ConstP.pooled138, 19U);
11585
11586 /* Lookup_n-D: '<S540>/table2D_Cl_dar' */
11587 _rtB->add_d = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11588 ->saturation_gs, AFCS_MODEL1_ConstP.pooled45,
11589 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled62,
11590 AFCS_MODEL1_ConstP.pooled138, 19U);
11591
11592 /* Lookup_n-D: '<S540>/table2D_Cl_dr' */
11593 _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11594 ->saturation_by, AFCS_MODEL1_ConstP.pooled45,
11595 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled63,
11596 AFCS_MODEL1_ConstP.pooled138, 19U);
11597
11598 /* Lookup_n-D: '<S540>/table2D_Cl_dfal' */
11599 _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,
11600 AFCS_MODEL1_rtC(S)->saturation_k0r, AFCS_MODEL1_ConstP.pooled45,
11601 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled64,
11602 AFCS_MODEL1_ConstP.pooled138, 19U);
11603
11604 /* Lookup_n-D: '<S540>/table2D_Cl_dfar' */
11605 _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC
11606 (S)->saturation_f, AFCS_MODEL1_ConstP.pooled45,
11607 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled65,
11608 AFCS_MODEL1_ConstP.pooled138, 19U);
11609
11610 /* Sum: '<S540>/add' */
11611 _rtB->add = (((_rtB->add + _rtB->add_d) + _rtB->add_e) +
11612 _rtB->table2D_Cn_dar) + _rtB->table2D_Cn_dr;
11613
11614 /* Lookup_n-D: '<S541>/table1D_Cm_base' */
11615 _rtB->add_d = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,
11616 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled66, 18U);
11617
11618 /* Lookup_n-D: '<S541>/table2D_Cm_de' */
11619 _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11620 ->saturation_ng, AFCS_MODEL1_ConstP.pooled45,
11621 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled67,
11622 AFCS_MODEL1_ConstP.pooled138, 19U);
11623
11624 /* Lookup_n-D: '<S541>/table2D_Cm_dal' */
11625 _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,
11626 AFCS_MODEL1_rtC(S)->saturation_g, AFCS_MODEL1_ConstP.pooled45,
11627 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled68,
11628 AFCS_MODEL1_ConstP.pooled138, 19U);
11629
11630 /* Lookup_n-D: '<S541>/table2D_Cm_dar' */
11631 _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC
11632 (S)->saturation_gs, AFCS_MODEL1_ConstP.pooled45,
11633 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled68,
11634 AFCS_MODEL1_ConstP.pooled138, 19U);
11635
11636 /* Lookup_n-D: '<S541>/table2D_Cm_dfal' */
11637 _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,
11638 AFCS_MODEL1_rtC(S)->saturation_k0r, AFCS_MODEL1_ConstP.pooled45,
11639 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled69,
11640 AFCS_MODEL1_ConstP.pooled138, 19U);
11641
11642 /* Lookup_n-D: '<S541>/table2D_Cm_dfar' */
11643 _rtB->table2D_Cm_dfar = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,
11644 AFCS_MODEL1_rtC(S)->saturation_f, AFCS_MODEL1_ConstP.pooled45,
11645 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled69,
11646 AFCS_MODEL1_ConstP.pooled138, 19U);
11647
11648 /* Lookup_n-D: '<S541>/table2D_Cm_dr' */
11649 _rtB->table2D_Cm_dr = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC
11650 (S)->saturation_by, AFCS_MODEL1_ConstP.pooled45,
11651 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled70,
11652 AFCS_MODEL1_ConstP.pooled138, 19U);
11653
11654 /* Sum: '<S541>/add' */
11655 _rtB->add_d = (((((_rtB->add_d + _rtB->add_e) + _rtB->table2D_Cn_dar) +
11656 _rtB->table2D_Cn_dr) + _rtB->table2D_Cn_dfal) +
11657 _rtB->table2D_Cm_dfar) + _rtB->table2D_Cm_dr;
11658
11659 /* Lookup_n-D: '<S542>/table2D_Cn_dal' */
11660 _rtB->add_e = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11661 ->saturation_g, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11662 AFCS_MODEL1_ConstP.pooled71, AFCS_MODEL1_ConstP.pooled138, 19U);
11663
11664 /* Lookup_n-D: '<S542>/table2D_Cn_dar' */
11665 _rtB->table2D_Cn_dar = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,
11666 AFCS_MODEL1_rtC(S)->saturation_gs, AFCS_MODEL1_ConstP.pooled45,
11667 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled72,
11668 AFCS_MODEL1_ConstP.pooled138, 19U);
11669
11670 /* Lookup_n-D: '<S542>/table2D_Cn_dr' */
11671 _rtB->table2D_Cn_dr = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC
11672 (S)->saturation_by, AFCS_MODEL1_ConstP.pooled45,
11673 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled73,
11674 AFCS_MODEL1_ConstP.pooled138, 19U);
11675
11676 /* Lookup_n-D: '<S542>/table2D_Cn_dfal' */
11677 _rtB->table2D_Cn_dfal = AFCS_MODEL1_look2_binlxpw(_rtB->jxk,
11678 AFCS_MODEL1_rtC(S)->saturation_k0r, AFCS_MODEL1_ConstP.pooled45,
11679 AFCS_MODEL1_ConstP.pooled47, AFCS_MODEL1_ConstP.pooled74,
11680 AFCS_MODEL1_ConstP.pooled138, 19U);
11681
11682 /* Lookup_n-D: '<S542>/table2D_Cn_dfar' */
11683 _rtB->jxk = AFCS_MODEL1_look2_binlxpw(_rtB->jxk, AFCS_MODEL1_rtC(S)
11684 ->saturation_f, AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled47,
11685 AFCS_MODEL1_ConstP.pooled75, AFCS_MODEL1_ConstP.pooled138, 19U);
11686
11687 /* Sum: '<S542>/add' */
11688 _rtB->add_e = (((_rtB->add_e + _rtB->table2D_Cn_dar) + _rtB->table2D_Cn_dr)
11689 + _rtB->table2D_Cn_dfal) + _rtB->jxk;
11690
11691 /* Sum: '<S490>/Subtract1' */
11692 _rtB->MatrixConcatenate1[12] = _rtB->kxi - _rtB->jxi_k;
11693 _rtB->MatrixConcatenate1[13] = _rtB->ixj - _rtB->jxi;
11694 _rtB->MatrixConcatenate1[14] = _rtB->kxj - _rtB->add_b;
11695 _rtB->MatrixConcatenate1[15] = _rtB->ixk_a - _rtB->add;
11696 _rtB->MatrixConcatenate1[16] = _rtB->jxi_p - _rtB->add_d;
11697 _rtB->MatrixConcatenate1[17] = _rtB->ixk - _rtB->add_e;
11698
11699 /* Selector: '<S490>/Selector' */
11700 _rtB->Product2_f[0] = _rtB->MatrixConcatenate1[0];
11701 _rtB->Product2_f[1] = _rtB->MatrixConcatenate1[6];
11702 _rtB->Product2_f[2] = _rtB->MatrixConcatenate1[12];
11703
11704 /* Dynamic Look-Up Table Block: '<S490>/Lookup Table Dynamic'
11705 * Input0 Data Type: Floating Point real_T
11706 * Input1 Data Type: Floating Point real_T
11707 * Input2 Data Type: Floating Point real_T
11708 * Output0 Data Type: Floating Point real_T
11709 * Lookup Method: Linear_Endpoint
11710 *
11711 */
11712 AFCS_MODEL1_sf_LookUpE_4Ol9QRGv( &(_rtB->jxk), &_rtB->Product2_f[0],
11713 AFCS_MODEL1_rtC(S)->Saturation, 0.0, 2U, 15.0);
11714
11715 /* Selector: '<S490>/Selector1' */
11716 _rtB->Product2_f[0] = _rtB->MatrixConcatenate1[2];
11717 _rtB->Product2_f[1] = _rtB->MatrixConcatenate1[8];
11718 _rtB->Product2_f[2] = _rtB->MatrixConcatenate1[14];
11719
11720 /* Dynamic Look-Up Table Block: '<S490>/Lookup Table Dynamic1'
11721 * Input0 Data Type: Floating Point real_T
11722 * Input1 Data Type: Floating Point real_T
11723 * Input2 Data Type: Floating Point real_T
11724 * Output0 Data Type: Floating Point real_T
11725 * Lookup Method: Linear_Endpoint
11726 *
11727 */
11728 AFCS_MODEL1_sf_LookUpE_4Ol9QRGv( &(_rtB->kxi), &_rtB->Product2_f[0],
11729 AFCS_MODEL1_rtC(S)->Saturation, 0.0, 2U, 15.0);
11730
11731 /* Selector: '<S490>/Selector2' */
11732 _rtB->Product2_f[0] = _rtB->MatrixConcatenate1[4];
11733 _rtB->Product2_f[1] = _rtB->MatrixConcatenate1[10];
11734 _rtB->Product2_f[2] = _rtB->MatrixConcatenate1[16];
11735
11736 /* Dynamic Look-Up Table Block: '<S490>/Lookup Table Dynamic2'
11737 * Input0 Data Type: Floating Point real_T
11738 * Input1 Data Type: Floating Point real_T
11739 * Input2 Data Type: Floating Point real_T
11740 * Output0 Data Type: Floating Point real_T
11741 * Lookup Method: Linear_Endpoint
11742 *
11743 */
11744 AFCS_MODEL1_sf_LookUpE_4Ol9QRGv( &(_rtB->ixj), &_rtB->Product2_f[0],
11745 AFCS_MODEL1_rtC(S)->Saturation, 0.0, 2U, 15.0);
11746
11747 /* Product: '<S571>/product' incorporates:
11748 * Constant: '<S571>/value'
11749 */
11750 _rtB->table2D_Cn *= 57.29;
11751
11752 /* Saturate: '<S571>/saturation' */
11753 if (_rtB->table2D_Cn > 30.0) {
11754 _rtB->table2D_Cn = 30.0;
11755 } else {
11756 if (_rtB->table2D_Cn < -6.0) {
11757 _rtB->table2D_Cn = -6.0;
11758 }
11759 }
11760
11761 /* End of Saturate: '<S571>/saturation' */
11762
11763 /* Product: '<S572>/product' incorporates:
11764 * Constant: '<S572>/value'
11765 */
11766 _rtB->saturation *= 57.29;
11767
11768 /* Saturate: '<S572>/saturation' */
11769 if (_rtB->saturation > 10.0) {
11770 _rtB->saturation = 10.0;
11771 } else {
11772 if (_rtB->saturation < -10.0) {
11773 _rtB->saturation = -10.0;
11774 }
11775 }
11776
11777 /* End of Saturate: '<S572>/saturation' */
11778
11779 /* Lookup_n-D: '<S565>/table2D_CD' */
11780 _rtB->kxj = AFCS_MODEL1_look2_binlxpw(_rtB->table2D_Cn, _rtB->saturation,
11781 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,
11782 AFCS_MODEL1_ConstP.table2D_CD_tableData_j, AFCS_MODEL1_ConstP.pooled139,
11783 19U);
11784
11785 /* Lookup_n-D: '<S567>/table2D_CY' */
11786 _rtB->ixk_a = AFCS_MODEL1_look2_binlxpw(_rtB->table2D_Cn, _rtB->saturation,
11787 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,
11788 AFCS_MODEL1_ConstP.table2D_CY_tableData_j, AFCS_MODEL1_ConstP.pooled139,
11789 19U);
11790
11791 /* Lookup_n-D: '<S566>/table2D_CL' */
11792 _rtB->jxi_p = AFCS_MODEL1_look2_binlxpw(_rtB->table2D_Cn, _rtB->saturation,
11793 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,
11794 AFCS_MODEL1_ConstP.table2D_CL_tableData_e, AFCS_MODEL1_ConstP.pooled139,
11795 19U);
11796
11797 /* Lookup_n-D: '<S568>/table2D_Cl' */
11798 _rtB->ixk = AFCS_MODEL1_look2_binlxpw(_rtB->table2D_Cn, _rtB->saturation,
11799 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,
11800 AFCS_MODEL1_ConstP.table2D_Cl_tableData_k, AFCS_MODEL1_ConstP.pooled139,
11801 19U);
11802
11803 /* Lookup_n-D: '<S569>/table2D_Cm' */
11804 _rtB->jxi_k = AFCS_MODEL1_look2_binlxpw(_rtB->table2D_Cn, _rtB->saturation,
11805 AFCS_MODEL1_ConstP.pooled45, AFCS_MODEL1_ConstP.pooled76,
11806 AFCS_MODEL1_ConstP.table2D_Cm_tableData_h, AFCS_MODEL1_ConstP.pooled139,
11807 19U);
11808
11809 /* Lookup_n-D: '<S570>/table2D_Cn' */
11810 _rtB->table2D_Cn = AFCS_MODEL1_look2_binlxpw(_rtB->table2D_Cn,
11811 _rtB->saturation, AFCS_MODEL1_ConstP.pooled45,
11812 AFCS_MODEL1_ConstP.pooled76, AFCS_MODEL1_ConstP.table2D_Cn_tableData_d,
11813 AFCS_MODEL1_ConstP.pooled139, 19U);
11814
11815 /* Sum: '<S490>/Sum' */
11816 _rtB->Sum1_h[0] = _rtB->jxk + _rtB->kxj;
11817 _rtB->Sum1_h[1] = _rtB->ixk_a;
11818 _rtB->Sum1_h[2] = _rtB->kxi + _rtB->jxi_p;
11819 _rtB->Sum1_h[3] = _rtB->ixk;
11820 _rtB->Sum1_h[4] = _rtB->ixj + _rtB->jxi_k;
11821 _rtB->Sum1_h[5] = _rtB->table2D_Cn;
11822 for (i_1 = 0; i_1 < 6; i_1++) {
11823 /* Sum: '<S487>/Sum' */
11824 _rtB->Sum2_g[i_1] += _rtB->Sum1_h[i_1];
11825 }
11826
11827 /* Gain: '<S488>/Gain1' */
11828 _rtB->jxk = -_rtB->Sum2_g[0];
11829
11830 /* Gain: '<S488>/Gain' */
11831 _rtB->kxi = -_rtB->Sum2_g[2];
11832
11833 /* SignalConversion: '<S488>/TmpSignal ConversionAtProductInport2' incorporates:
11834 * Constant: '<S488>/Constant'
11835 */
11836 _rtB->Product2_f[0] = _rtB->jxk;
11837 _rtB->Product2_f[1] = 0.0;
11838 _rtB->Product2_f[2] = _rtB->kxi;
11839 for (i_1 = 0; i_1 < 3; i_1++) {
11840 /* Product: '<S488>/Product' */
11841 _rtB->Sum4[i_1] = 0.0;
11842 _rtB->Sum4[i_1] += _rtB->MathFunction1_a[i_1] * _rtB->Product2_f[0];
11843 _rtB->Sum4[i_1] += _rtB->MathFunction1_a[i_1 + 3] * _rtB->Product2_f[1];
11844 _rtB->Sum4[i_1] += _rtB->MathFunction1_a[i_1 + 6] * _rtB->Product2_f[2];
11845
11846 /* Sum: '<S486>/Sum' */
11847 tmp[i_1 + 3] = _rtB->Sum2_g[3 + i_1];
11848 }
11849
11850 /* Sum: '<S486>/Sum' */
11851 tmp[0] = _rtB->Sum4[0];
11852 tmp[1] = _rtB->Sum2_g[1];
11853 tmp[2] = _rtB->Sum4[2];
11854 for (i_1 = 0; i_1 < 6; i_1++) {
11855 _rtB->Sum2_g[i_1] = tmp[i_1];
11856 }
11857
11858 /* Gain: '<S480>/Gain' */
11859 _rtB->Sum4[0] = 4.785 * _rtB->Sum2_g[3];
11860
11861 /* Product: '<S480>/Product5' */
11862 _rtB->Sum4[0] *= _rtB->Product4_n;
11863
11864 /* Product: '<S480>/Product2' */
11865 _rtB->Product2_f[0] = _rtB->Sum2_g[0] * _rtB->Product4_n;
11866
11867 /* Gain: '<S480>/Gain' */
11868 _rtB->Sum4[1] = 0.648 * _rtB->Sum2_g[4];
11869
11870 /* Product: '<S480>/Product5' */
11871 _rtB->Sum4[1] *= _rtB->Product4_n;
11872
11873 /* Product: '<S480>/Product2' */
11874 _rtB->Product2_f[1] = _rtB->Sum2_g[1] * _rtB->Product4_n;
11875
11876 /* Gain: '<S480>/Gain' */
11877 _rtB->Sum4[2] = 4.785 * _rtB->Sum2_g[5];
11878
11879 /* Product: '<S480>/Product5' */
11880 _rtB->Sum4[2] *= _rtB->Product4_n;
11881
11882 /* Product: '<S480>/Product2' */
11883 _rtB->Product2_f[2] = _rtB->Sum2_g[2] * _rtB->Product4_n;
11884
11885 /* Product: '<S601>/Element product' */
11886 _rtB->Sum1_h[0] = AFCS_MODEL1_rtC(S)->Gain2[1] * _rtB->Product2_f[2];
11887 _rtB->Sum1_h[1] = AFCS_MODEL1_rtC(S)->Gain2[2] * _rtB->Product2_f[0];
11888 _rtB->Sum1_h[2] = AFCS_MODEL1_rtC(S)->Gain2[0] * _rtB->Product2_f[1];
11889 _rtB->Sum1_h[3] = AFCS_MODEL1_rtC(S)->Gain2[2] * _rtB->Product2_f[1];
11890 _rtB->Sum1_h[4] = AFCS_MODEL1_rtC(S)->Gain2[0] * _rtB->Product2_f[2];
11891 _rtB->Sum1_h[5] = AFCS_MODEL1_rtC(S)->Gain2[1] * _rtB->Product2_f[0];
11892
11893 /* Sum: '<S601>/Add3' */
11894 _rtB->Product2_p[0] = _rtB->Sum1_h[0] - _rtB->Sum1_h[3];
11895
11896 /* Sum: '<S485>/Sum1' */
11897 _rtB->Sum1[0] = _rtB->Sum4[0] + _rtB->Product2_p[0];
11898
11899 /* Sum: '<S601>/Add3' */
11900 _rtB->Product2_p[1] = _rtB->Sum1_h[1] - _rtB->Sum1_h[4];
11901
11902 /* Sum: '<S485>/Sum1' */
11903 _rtB->Sum1[1] = _rtB->Sum4[1] + _rtB->Product2_p[1];
11904
11905 /* Sum: '<S601>/Add3' */
11906 _rtB->Product2_p[2] = _rtB->Sum1_h[2] - _rtB->Sum1_h[5];
11907
11908 /* Sum: '<S485>/Sum1' */
11909 _rtB->Sum1[2] = _rtB->Sum4[2] + _rtB->Product2_p[2];
11910 }
11911
11912 if (ssIsSampleHit(S, 1, 0)) {
11913 }
11914
11915 if (1) {
11916 /* Backlash: '<S446>/Backlash' */
11917 if (_rtB->DataTypeConversion4[7] < ((real_T *)ssGetDWork(S, 4))[0] - 1.0)
11918 {
11919 _rtB->Backlash = _rtB->DataTypeConversion4[7] + 1.0;
11920 } else if (_rtB->DataTypeConversion4[7] <= ((real_T *)ssGetDWork(S, 4))[0]
11921 + 1.0) {
11922 _rtB->Backlash = ((real_T *)ssGetDWork(S, 4))[0];
11923 } else {
11924 _rtB->Backlash = _rtB->DataTypeConversion4[7] - 1.0;
11925 }
11926
11927 /* End of Backlash: '<S446>/Backlash' */
11928
11929 /* Gain: '<S469>/Gain2' */
11930 _rtB->jxk = 57.3 * _rtB->Merge_n[2];
11931
11932 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
11933 _rtB->phiL = _rtB->jxk;
11934
11935 /* Gain: '<S446>/Gain' */
11936 _rtB->Lat_p = _rtB->UnitConversion_c4[0];
11937
11938 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
11939 _rtB->Lat_j = _rtB->Lat_p;
11940
11941 /* Gain: '<S446>/Gain1' */
11942 _rtB->Lon_j = _rtB->UnitConversion_c4[1];
11943
11944 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
11945 _rtB->Lon_e = _rtB->Lon_j;
11946
11947 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
11948 _rtB->Altitude_j = _rtB->Alt;
11949
11950 /* Math: '<S469>/Math Function' */
11951 for (i_1 = 0; i_1 < 3; i_1++) {
11952 _rtB->MathFunction1_a[3 * i_1] = _rtB->Product4[i_1];
11953 _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->Product4[i_1 + 3];
11954 _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->Product4[i_1 + 6];
11955 }
11956
11957 /* End of Math: '<S469>/Math Function' */
11958 for (i_1 = 0; i_1 < 3; i_1++) {
11959 /* Product: '<S469>/Product' */
11960 _rtB->Product2_p[i_1] = 0.0;
11961
11962 /* Product: '<S470>/Product3' */
11963 _rtB->Sum4[i_1] = 0.0;
11964
11965 /* Product: '<S469>/Product' */
11966 _rtB->Product2_p[i_1] += _rtB->MathFunction1_a[i_1] *
11967 _rtB->UnitConversion_o[0];
11968
11969 /* Product: '<S470>/Product3' */
11970 _rtB->Sum4[i_1] += _rtB->VectorConcatenate_n[i_1] * 0.0;
11971
11972 /* Product: '<S469>/Product' */
11973 _rtB->Product2_p[i_1] += _rtB->MathFunction1_a[i_1 + 3] *
11974 _rtB->UnitConversion_o[1];
11975
11976 /* Product: '<S470>/Product3' */
11977 _rtB->Sum4[i_1] += _rtB->VectorConcatenate_n[i_1 + 3] * 0.0;
11978
11979 /* Product: '<S469>/Product' */
11980 _rtB->Product2_p[i_1] += _rtB->MathFunction1_a[i_1 + 6] *
11981 _rtB->UnitConversion_o[2];
11982
11983 /* Product: '<S470>/Product3' */
11984 _rtB->Sum4[i_1] += _rtB->VectorConcatenate_n[i_1 + 6] * 7.292115E-5;
11985 }
11986
11987 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
11988 _rtB->GPS_vnorth_i = _rtB->Product2_p[0];
11989
11990 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
11991 _rtB->GPS_veast_k = _rtB->Product2_p[1];
11992
11993 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
11994 _rtB->GPS_vdown_k = _rtB->Product2_p[2];
11995
11996 /* Gain: '<S469>/Gain3' */
11997 _rtB->jxk = 57.3 * _rtB->Merge_n[1];
11998
11999 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
12000 _rtB->thetaL = _rtB->jxk;
12001
12002 /* Gain: '<S767>/Unit Conversion' */
12003 _rtB->jxk = 57.295779513082323 * _rtB->Switch_h;
12004
12005 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
12006 _rtB->Alfa = _rtB->jxk;
12007
12008 /* Gain: '<S768>/Unit Conversion' */
12009 _rtB->UnitConversion_j *= 57.295779513082323;
12010
12011 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
12012 _rtB->Beta = _rtB->UnitConversion_j;
12013
12014 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
12015 _rtB->Qbar = _rtB->u2rhoV2;
12016
12017 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
12018 _rtB->Va = _rtB->Airspeed;
12019
12020 /* Gain: '<S469>/Gain4' */
12021 _rtB->jxk = 57.3 * _rtB->Merge_n[0];
12022
12023 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
12024 _rtB->psiL = _rtB->jxk;
12025
12026 /* Math: '<S783>/Math Function2' */
12027 for (i_1 = 0; i_1 < 3; i_1++) {
12028 _rtB->MathFunction1_a[3 * i_1] = _rtB->Product4[i_1];
12029 _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->Product4[i_1 + 3];
12030 _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->Product4[i_1 + 6];
12031 }
12032
12033 /* End of Math: '<S783>/Math Function2' */
12034
12035 /* Product: '<S783>/Product3' */
12036 for (i_1 = 0; i_1 < 3; i_1++) {
12037 _rtB->Sum2_h[i_1] = 0.0;
12038 _rtB->Sum2_h[i_1] += _rtB->MathFunction1_a[i_1] * _rtB->ubvbwb[0];
12039 _rtB->Sum2_h[i_1] += _rtB->MathFunction1_a[i_1 + 3] * _rtB->ubvbwb[1];
12040 _rtB->Sum2_h[i_1] += _rtB->MathFunction1_a[i_1 + 6] * _rtB->ubvbwb[2];
12041 }
12042
12043 /* End of Product: '<S783>/Product3' */
12044
12045 /* Gain: '<S950>/Unit Conversion' */
12046 _rtB->jxk = 0.017453292519943295 * _rtB->UnitConversion_c4[0];
12047
12048 /* Trigonometry: '<S783>/sine' */
12049 _rtB->kxi = sin(_rtB->jxk);
12050
12051 /* Product: '<S783>/Product' */
12052 _rtB->kxi = _rtB->kxi * _rtB->kxi * AFCS_MODEL1_rtC(S)->Sum2;
12053
12054 /* Sum: '<S783>/Sum2' incorporates:
12055 * Constant: '<S783>/f2'
12056 */
12057 _rtB->kxi = 1.0 - _rtB->kxi;
12058
12059 /* Sqrt: '<S952>/sqrt' */
12060 _rtB->ixj = sqrt(_rtB->kxi);
12061
12062 /* Product: '<S952>/N' incorporates:
12063 * Constant: '<S952>/f4'
12064 */
12065 _rtB->ixj = 6.371E+6 / _rtB->ixj;
12066
12067 /* Sum: '<S952>/Sum4' */
12068 _rtB->ixj += _rtB->Sum3_j;
12069
12070 /* Product: '<S783>/w1' */
12071 _rtB->ixj = _rtB->Sum2_h[1] / _rtB->ixj;
12072
12073 /* Math: '<S951>/Math Function' incorporates:
12074 * Constant: '<S951>/f3'
12075 */
12076 if (_rtB->kxi < 0.0) {
12077 _rtB->kxi = -AFCS_MODEL1_rt_powd_snf(-_rtB->kxi, 1.5);
12078 } else {
12079 _rtB->kxi = AFCS_MODEL1_rt_powd_snf(_rtB->kxi, 1.5);
12080 }
12081
12082 /* End of Math: '<S951>/Math Function' */
12083
12084 /* Product: '<S951>/M' incorporates:
12085 * Constant: '<S951>/f1'
12086 */
12087 _rtB->kxi = 6.371E+6 * AFCS_MODEL1_rtC(S)->Sum_ha / _rtB->kxi;
12088
12089 /* Sum: '<S951>/Sum1' */
12090 _rtB->kxi += _rtB->Sum3_j;
12091
12092 /* Product: '<S783>/w2' */
12093 _rtB->kxi = _rtB->Sum2_h[0] / _rtB->kxi;
12094
12095 /* Gain: '<S783>/Gain' */
12096 _rtB->kxi = -_rtB->kxi;
12097
12098 /* Trigonometry: '<S783>/tan' */
12099 _rtB->jxk = tan(_rtB->jxk);
12100
12101 /* Product: '<S783>/w3' */
12102 _rtB->jxk *= _rtB->ixj;
12103
12104 /* Gain: '<S783>/Gain1' */
12105 _rtB->jxk = -_rtB->jxk;
12106
12107 /* SignalConversion: '<S470>/TmpSignal ConversionAtProduct2Inport2' */
12108 _rtB->Sum2_h[0] = _rtB->ixj;
12109 _rtB->Sum2_h[1] = _rtB->kxi;
12110 _rtB->Sum2_h[2] = _rtB->jxk;
12111 for (i_1 = 0; i_1 < 3; i_1++) {
12112 /* Product: '<S470>/Product2' */
12113 _rtB->Sum1_d[i_1] = 0.0;
12114 _rtB->Sum1_d[i_1] += _rtB->Product4[i_1] * _rtB->Sum2_h[0];
12115 _rtB->Sum1_d[i_1] += _rtB->Product4[i_1 + 3] * _rtB->Sum2_h[1];
12116 _rtB->Sum1_d[i_1] += _rtB->Product4[i_1 + 6] * _rtB->Sum2_h[2];
12117
12118 /* Sum: '<S470>/Sum3' */
12119 _rtB->Sum1_d[i_1] += _rtB->Sum4[i_1];
12120
12121 /* Sum: '<S470>/Sum2' */
12122 _rtB->Sum2_op[i_1] = _rtB->Sum1_d[i_1];
12123 }
12124
12125 /* Integrator: '<S470>/p,q,r ' */
12126 if (((int_T *)ssGetDWork(S, 347))[0] != 0) {
12127 _rtX->pqr_CSTATE[0] = _rtB->Sum2_op[0];
12128 _rtX->pqr_CSTATE[1] = _rtB->Sum2_op[1];
12129 _rtX->pqr_CSTATE[2] = _rtB->Sum2_op[2];
12130 }
12131
12132 _rtB->Sum2_h[0] = _rtX->pqr_CSTATE[0];
12133
12134 /* Sum: '<S470>/Sum1' */
12135 _rtB->Sum1_d[0] = _rtB->Sum2_h[0] - _rtB->Sum1_d[0];
12136
12137 /* Integrator: '<S470>/p,q,r ' */
12138 _rtB->Sum2_h[1] = _rtX->pqr_CSTATE[1];
12139
12140 /* Sum: '<S470>/Sum1' */
12141 _rtB->Sum1_d[1] = _rtB->Sum2_h[1] - _rtB->Sum1_d[1];
12142
12143 /* Integrator: '<S470>/p,q,r ' */
12144 _rtB->Sum2_h[2] = _rtX->pqr_CSTATE[2];
12145
12146 /* Sum: '<S470>/Sum1' */
12147 _rtB->Sum1_d[2] = _rtB->Sum2_h[2] - _rtB->Sum1_d[2];
12148
12149 /* Gain: '<S469>/Gain5' */
12150 _rtB->jxk = 57.3 * _rtB->Sum1_d[0];
12151
12152 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
12153 _rtB->p = _rtB->jxk;
12154
12155 /* Gain: '<S469>/Gain7' */
12156 _rtB->jxk = 57.3 * _rtB->Sum1_d[1];
12157
12158 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
12159 _rtB->q = _rtB->jxk;
12160
12161 /* Gain: '<S469>/Gain8' */
12162 _rtB->jxk = 57.3 * _rtB->Sum1_d[2];
12163
12164 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
12165 _rtB->r = _rtB->jxk;
12166
12167 /* SignalConversion: '<S954>/TmpSignal ConversionAtSelectorInport1' */
12168 _rtB->p_p[0] = _rtB->Lat_p;
12169 _rtB->p_p[1] = _rtB->Lon_j;
12170 _rtB->p_p[2] = _rtB->Alt;
12171
12172 /* Selector: '<S954>/Selector2' */
12173 _rtB->jxk = _rtB->p_p[0];
12174
12175 /* Gain: '<S956>/Unit Conversion' */
12176 _rtB->jxk *= 0.017453292519943295;
12177
12178 /* Selector: '<S954>/Selector1' */
12179 _rtB->kxi = _rtB->p_p[1];
12180
12181 /* Gain: '<S957>/Unit Conversion' */
12182 _rtB->kxi *= 0.017453292519943295;
12183
12184 /* Selector: '<S954>/Selector' */
12185 _rtB->ixj = _rtB->p_p[2];
12186
12187 /* S-Function Block: <S954>/WGS84 Gravity S-Function */
12188 {
12189 int_T i;
12190 real_T GM, opt_m2ft, deg2rad;
12191 real_T *phi_ptr, *height_ptr;
12192 boolean_T phi_wrapped = false;
12193 real_T *phi = (real_T *) &((real_T*) ssGetDWork(S, 6))[0];
12194 real_T *h = (real_T *) &((real_T*) ssGetDWork(S, 5))[0];
12195 height_ptr = (real_T *) &_rtB->ixj;
12196 phi_ptr = (real_T *) &_rtB->jxk;
12197
12198 /* get unit conversion factor */
12199 opt_m2ft = 1.0;
12200 deg2rad = 1.0;
12201
12202 /* Use Earth's Atmosphere in Gravitational Const? */
12203 GM = ( 1.0 == 0 ) ? WGS84_GM_DEF : WGS84_GM_PRM;
12204
12205 /* WGS84TAYLORSERIES */
12206 {
12207 for (i = 0; i < 1; i++ ) {
12208 real_T fphi, pi_2;
12209
12210 /* create short variables for latitude (phi) and height (h) */
12211 phi[i] = phi_ptr[i] * deg2rad;
12212 h[i] = height_ptr[i] / opt_m2ft;
12213 if (phi[i] > AERO_PI || phi[i] < -AERO_PI) {
12214 phi[i] = fmod(phi[i]+AERO_PI, 2.0*AERO_PI) - AERO_PI;
12215 }
12216
12217 fphi = fabs(phi[i]);
12218 pi_2 = AERO_PI/2.0;
12219 if (fphi > pi_2 ) {
12220 real_T sign = phi[i]/fphi;
12221 phi_wrapped = true;
12222 phi[i] = sign*(pi_2 - (fphi - pi_2));
12223 } else {
12224 phi_wrapped = false;
12225 }
12226 }
12227
12228 wgs84_taylor_series(h, phi, opt_m2ft, &_rtB->MatrixConcatenate[2], 1);
12229
12230 /* north and east components are zero */
12231 _rtB->MatrixConcatenate[0] = ((real_T*) ssGetDWork(S, 9))[0];
12232 _rtB->MatrixConcatenate[1] = ((real_T*) ssGetDWork(S, 9))[0];
12233 }
12234 }
12235
12236 /* Math: '<S471>/Math Function' */
12237 _rtB->p_p[0] = _rtB->MatrixConcatenate[0];
12238 _rtB->p_p[1] = _rtB->MatrixConcatenate[1];
12239 _rtB->p_p[2] = _rtB->MatrixConcatenate[2];
12240
12241 /* Gain: '<S771>/Unit Conversion' */
12242 _rtB->jxk = 0.017453292519943295 * _rtB->Lat_p;
12243
12244 /* Trigonometry: '<S770>/Trigonometric Function1' */
12245 _rtB->kxi = sin(_rtB->jxk);
12246
12247 /* Math: '<S770>/Math Function' */
12248 _rtB->ixj = _rtB->kxi * _rtB->kxi;
12249
12250 /* Product: '<S770>/Product6' incorporates:
12251 * Constant: '<S770>/Constant1'
12252 */
12253 _rtB->ixj *= 0.0;
12254
12255 /* Sum: '<S770>/Sum2' incorporates:
12256 * Constant: '<S770>/Constant2'
12257 */
12258 _rtB->ixj = 1.0 - _rtB->ixj;
12259
12260 /* Product: '<S770>/Divide2' incorporates:
12261 * Constant: '<S770>/Constant'
12262 */
12263 _rtB->ixj /= 6.371E+6;
12264
12265 /* Product: '<S770>/Divide1' */
12266 _rtB->ixj *= _rtB->Product2_p[1];
12267
12268 /* Math: '<S770>/Math Function1' */
12269 _rtB->kxi *= _rtB->kxi;
12270
12271 /* Product: '<S770>/Product7' incorporates:
12272 * Constant: '<S770>/Constant4'
12273 */
12274 _rtB->kxi *= 0.0;
12275
12276 /* Gain: '<S770>/Gain4' */
12277 _rtB->kxi *= 3.0;
12278
12279 /* Sum: '<S770>/Sum3' incorporates:
12280 * Constant: '<S770>/Constant5'
12281 */
12282 _rtB->kxi = (AFCS_MODEL1_rtC(S)->Gain3 + 1.0) - _rtB->kxi;
12283
12284 /* Product: '<S770>/Divide3' incorporates:
12285 * Constant: '<S770>/Constant3'
12286 */
12287 _rtB->kxi /= 6.371E+6;
12288
12289 /* Product: '<S770>/Divide' */
12290 _rtB->kxi *= _rtB->Product2_p[0];
12291
12292 /* Gain: '<S770>/Gain1' */
12293 _rtB->kxi = -_rtB->kxi;
12294
12295 /* Trigonometry: '<S770>/Trigonometric Function' */
12296 _rtB->jxk = tan(_rtB->jxk);
12297
12298 /* Product: '<S770>/Product4' */
12299 _rtB->jxk *= _rtB->ixj;
12300
12301 /* Gain: '<S770>/Gain2' */
12302 _rtB->jxk = -_rtB->jxk;
12303
12304 /* SignalConversion: '<S770>/TmpSignal ConversionAtProduct5Inport2' */
12305 _rtB->Product2_p[0] = _rtB->ixj;
12306 _rtB->Product2_p[1] = _rtB->kxi;
12307 _rtB->Product2_p[2] = _rtB->jxk;
12308 for (i_1 = 0; i_1 < 3; i_1++) {
12309 /* Product: '<S770>/Product1' */
12310 _rtB->Product1_f[i_1] = 0.0;
12311
12312 /* Product: '<S770>/Product5' */
12313 _rtB->Sum_mx[i_1] = 0.0;
12314
12315 /* Product: '<S770>/Product1' */
12316 _rtB->Product1_f[i_1] += _rtB->Product4[i_1] * _rtB->p_p[0];
12317
12318 /* Product: '<S770>/Product5' */
12319 _rtB->Sum_mx[i_1] += _rtB->Product4[i_1] * _rtB->Product2_p[0];
12320
12321 /* Product: '<S770>/Product1' */
12322 _rtB->Product1_f[i_1] += _rtB->Product4[i_1 + 3] * _rtB->p_p[1];
12323
12324 /* Product: '<S770>/Product5' */
12325 _rtB->Sum_mx[i_1] += _rtB->Product4[i_1 + 3] * _rtB->Product2_p[1];
12326
12327 /* Product: '<S770>/Product1' */
12328 _rtB->Product1_f[i_1] += _rtB->Product4[i_1 + 6] * _rtB->p_p[2];
12329
12330 /* Product: '<S770>/Product5' */
12331 _rtB->Sum_mx[i_1] += _rtB->Product4[i_1 + 6] * _rtB->Product2_p[2];
12332 }
12333
12334 /* Product: '<S770>/Product2' */
12335 for (i_1 = 0; i_1 < 3; i_1++) {
12336 _rtB->Product2_p[i_1] = 0.0;
12337 _rtB->Product2_p[i_1] += _rtB->VectorConcatenate_nn[i_1] * 0.0;
12338 _rtB->Product2_p[i_1] += _rtB->VectorConcatenate_nn[i_1 + 3] * 0.0;
12339 _rtB->Product2_p[i_1] += _rtB->VectorConcatenate_nn[i_1 + 6] *
12340 7.292115E-5;
12341 }
12342
12343 /* End of Product: '<S770>/Product2' */
12344 for (i_1 = 0; i_1 < 3; i_1++) {
12345 /* Product: '<S770>/Product3' */
12346 _rtB->Product_d[i_1] = 0.0;
12347 _rtB->Product_d[i_1] += _rtB->Product4[i_1] * _rtB->Product2_p[0];
12348 _rtB->Product_d[i_1] += _rtB->Product4[i_1 + 3] * _rtB->Product2_p[1];
12349 _rtB->Product_d[i_1] += _rtB->Product4[i_1 + 6] * _rtB->Product2_p[2];
12350
12351 /* Gain: '<S770>/Gain' */
12352 _rtB->Product_d[i_1] *= 2.0;
12353
12354 /* Sum: '<S770>/Sum' */
12355 _rtB->Sum_mx[i_1] += _rtB->Product_d[i_1];
12356 }
12357
12358 /* Product: '<S772>/Element product' */
12359 _rtB->Sum1_h[0] = _rtB->Sum_mx[1] * _rtB->UnitConversion_o[2];
12360 _rtB->Sum1_h[1] = _rtB->Sum_mx[2] * _rtB->UnitConversion_o[0];
12361 _rtB->Sum1_h[2] = _rtB->Sum_mx[0] * _rtB->UnitConversion_o[1];
12362 _rtB->Sum1_h[3] = _rtB->Sum_mx[2] * _rtB->UnitConversion_o[1];
12363 _rtB->Sum1_h[4] = _rtB->Sum_mx[0] * _rtB->UnitConversion_o[2];
12364 _rtB->Sum1_h[5] = _rtB->Sum_mx[1] * _rtB->UnitConversion_o[0];
12365
12366 /* Sum: '<S772>/Add3' */
12367 _rtB->Product_d[0] = _rtB->Sum1_h[0] - _rtB->Sum1_h[3];
12368
12369 /* Sum: '<S770>/Sum1' */
12370 _rtB->Product1_f[0] += _rtB->Product_d[0];
12371
12372 /* Product: '<S471>/Product' */
12373 _rtB->p_p[0] *= AFCS_MODEL1_rtC(S)->Product4_d;
12374
12375 /* Sum: '<S772>/Add3' */
12376 _rtB->Product_d[1] = _rtB->Sum1_h[1] - _rtB->Sum1_h[4];
12377
12378 /* Sum: '<S770>/Sum1' */
12379 _rtB->Product1_f[1] += _rtB->Product_d[1];
12380
12381 /* Product: '<S471>/Product' */
12382 _rtB->p_p[1] *= AFCS_MODEL1_rtC(S)->Product4_d;
12383
12384 /* Sum: '<S772>/Add3' */
12385 _rtB->Product_d[2] = _rtB->Sum1_h[2] - _rtB->Sum1_h[5];
12386
12387 /* Sum: '<S770>/Sum1' */
12388 _rtB->Product1_f[2] += _rtB->Product_d[2];
12389
12390 /* Product: '<S471>/Product' */
12391 _rtB->p_p[2] *= AFCS_MODEL1_rtC(S)->Product4_d;
12392
12393 /* Saturate: '<S468>/Saturation' incorporates:
12394 * Saturate: '<S468>/Saturation1'
12395 */
12396 if (_rtB->Gain6_k > 1.0) {
12397 _rtB->jxk = 1.0;
12398 _rtB->Gain6_k = 1.0;
12399 } else {
12400 if (_rtB->Gain6_k < 0.0) {
12401 _rtB->jxk = 0.0;
12402 } else {
12403 _rtB->jxk = _rtB->Gain6_k;
12404 }
12405
12406 if (_rtB->Gain6_k < 0.0) {
12407 _rtB->Gain6_k = 0.0;
12408 }
12409 }
12410
12411 /* End of Saturate: '<S468>/Saturation' */
12412
12413 /* Lookup_n-D: '<S764>/eng_T' */
12414 _rtB->jxk = AFCS_MODEL1_look1_binlxpw(_rtB->jxk,
12415 AFCS_MODEL1_ConstP.pooled24, AFCS_MODEL1_ConstP.pooled89, 1U);
12416
12417 /* Product: '<S754>/Product3' */
12418 _rtB->kxi = AFCS_MODEL1_rtC(S)->TrigonometricFunction1 * _rtB->jxk *
12419 AFCS_MODEL1_rtC(S)->TrigonometricFunction2;
12420
12421 /* Product: '<S754>/Product2' */
12422 _rtB->ixj = _rtB->jxk * AFCS_MODEL1_rtC(S)->TrigonometricFunction;
12423
12424 /* Product: '<S754>/Product4' */
12425 _rtB->jxk = AFCS_MODEL1_rtC(S)->TrigonometricFunction1 * _rtB->jxk *
12426 AFCS_MODEL1_rtC(S)->TrigonometricFunction3;
12427
12428 /* Gain: '<S754>/Gain6' */
12429 _rtB->jxk = -_rtB->jxk;
12430
12431 /* Lookup_n-D: '<S758>/eng_T' */
12432 _rtB->Gain6_k = AFCS_MODEL1_look1_binlxpw(_rtB->Gain6_k,
12433 AFCS_MODEL1_ConstP.pooled24, AFCS_MODEL1_ConstP.pooled89, 1U);
12434
12435 /* Product: '<S753>/Product3' */
12436 _rtB->kxj = AFCS_MODEL1_rtC(S)->TrigonometricFunction1_h * _rtB->Gain6_k *
12437 AFCS_MODEL1_rtC(S)->TrigonometricFunction2_b;
12438
12439 /* Product: '<S753>/Product2' */
12440 _rtB->ixk_a = _rtB->Gain6_k * AFCS_MODEL1_rtC(S)->TrigonometricFunction_d;
12441
12442 /* Product: '<S753>/Product4' */
12443 _rtB->Gain6_k = AFCS_MODEL1_rtC(S)->TrigonometricFunction1_h *
12444 _rtB->Gain6_k * AFCS_MODEL1_rtC(S)->TrigonometricFunction3_d;
12445
12446 /* Gain: '<S753>/Gain6' */
12447 _rtB->Gain6_k = -_rtB->Gain6_k;
12448
12449 /* Sum: '<S468>/Add' */
12450 _rtB->Sum1_h[0] = _rtB->kxi + _rtB->kxj;
12451 _rtB->Sum1_h[1] = _rtB->ixj + _rtB->ixk_a;
12452 _rtB->Sum1_h[2] = _rtB->jxk + _rtB->Gain6_k;
12453 for (i_1 = 0; i_1 < 3; i_1++) {
12454 /* Product: '<S471>/Product1' */
12455 _rtB->Product_d[i_1] = 0.0;
12456 _rtB->Product_d[i_1] += _rtB->Product4[i_1] * _rtB->p_p[0];
12457 _rtB->Product_d[i_1] += _rtB->Product4[i_1 + 3] * _rtB->p_p[1];
12458 _rtB->Product_d[i_1] += _rtB->Product4[i_1 + 6] * _rtB->p_p[2];
12459
12460 /* Sum: '<S468>/Add' */
12461 _rtB->Sum1_h[i_1 + 3] = AFCS_MODEL1_rtC(S)->Sum3[i_1] + AFCS_MODEL1_rtC
12462 (S)->Sum3_k[i_1];
12463 }
12464
12465 /* Product: '<S617>/Product13' incorporates:
12466 * Constant: '<S617>/Constant4'
12467 */
12468 _rtB->jxk = 0.002 * _rtB->UnitConversion_o[0];
12469
12470 /* Gain: '<S617>/Gain' */
12471 _rtB->jxk = -_rtB->jxk;
12472
12473 /* Math: '<S617>/Math Function'
12474 *
12475 * About '<S617>/Math Function':
12476 * Operator: exp
12477 */
12478 _rtB->jxk = exp(_rtB->jxk);
12479
12480 /* Product: '<S617>/Product12' incorporates:
12481 * Constant: '<S617>/Constant3'
12482 */
12483 _rtB->jxk *= 0.23;
12484
12485 /* Product: '<S617>/Product14' */
12486 _rtB->Product14 = _rtB->jxk * 0.0;
12487
12488 /* Sum: '<S617>/Add' incorporates:
12489 * Constant: '<S617>/Constant1'
12490 */
12491 _rtB->kxi = _rtB->Product14 + 0.02;
12492
12493 /* Switch: '<S617>/Switch' incorporates:
12494 * Constant: '<S617>/Constant8'
12495 */
12496 if (_rtB->UnitConversion_o[0] >= 0.1) {
12497 _rtB->ixj = 1.0;
12498 } else {
12499 /* Product: '<S617>/Divide3' incorporates:
12500 * Constant: '<S617>/Constant9'
12501 */
12502 _rtB->Divide3 = _rtB->UnitConversion_o[0] / 0.1;
12503 _rtB->ixj = _rtB->Divide3;
12504 }
12505
12506 /* End of Switch: '<S617>/Switch' */
12507
12508 /* Product: '<S617>/Product5' */
12509 _rtB->kxi *= _rtB->ixj;
12510
12511 /* Saturate: '<S617>/Saturation4' */
12512 if (_rtB->kxi > 0.7) {
12513 _rtB->Saturation4 = 0.7;
12514 } else if (_rtB->kxi < 0.0) {
12515 _rtB->Saturation4 = 0.0;
12516 } else {
12517 _rtB->Saturation4 = _rtB->kxi;
12518 }
12519
12520 /* End of Saturate: '<S617>/Saturation4' */
12521
12522 /* Math: '<S617>/Math Function7' */
12523 _rtB->kxi = _rtB->Saturation4 * _rtB->Saturation4;
12524
12525 /* Sum: '<S617>/Subtract1' */
12526 _rtB->kxi = AFCS_MODEL1_rtC(S)->MathFunction6 - _rtB->kxi;
12527
12528 /* Sqrt: '<S617>/Sqrt1' */
12529 _rtB->kxi = sqrt(_rtB->kxi);
12530
12531 /* SignalConversion: '<S628>/TmpSignal ConversionAtSinCosInport1' incorporates:
12532 * Constant: '<S617>/Constant13'
12533 */
12534 _rtB->Sum_mx[0] = _rtB->Merge_n[2];
12535 _rtB->Sum_mx[1] = _rtB->Merge_n[1];
12536 _rtB->Sum_mx[2] = 0.0;
12537
12538 /* Trigonometry: '<S628>/SinCos' */
12539 TrigonometricFunction4_o = cos(_rtB->Sum_mx[0]);
12540 _rtB->Sum_mx[0] = sin(_rtB->Sum_mx[0]);
12541 _rtB->p_p[0] = TrigonometricFunction4_o;
12542 TrigonometricFunction4_o = cos(_rtB->Sum_mx[1]);
12543 _rtB->Sum_mx[1] = sin(_rtB->Sum_mx[1]);
12544 _rtB->p_p[1] = TrigonometricFunction4_o;
12545 TrigonometricFunction4_o = cos(_rtB->Sum_mx[2]);
12546 _rtB->Sum_mx[2] = sin(_rtB->Sum_mx[2]);
12547 _rtB->p_p[2] = TrigonometricFunction4_o;
12548
12549 /* Product: '<S641>/u(5)*u(6)' */
12550 _rtB->VectorConcatenate_mr[0] = _rtB->p_p[1] * _rtB->p_p[2];
12551
12552 /* Product: '<S644>/u(6)*u(1)*u(2)' */
12553 _rtB->kxj = _rtB->p_p[2] * _rtB->Sum_mx[0] * _rtB->Sum_mx[1];
12554
12555 /* Product: '<S644>/u(3)*u(4)' */
12556 _rtB->ixk_a = _rtB->Sum_mx[2] * _rtB->p_p[0];
12557
12558 /* Sum: '<S644>/Sum' */
12559 _rtB->VectorConcatenate_mr[1] = _rtB->kxj - _rtB->ixk_a;
12560
12561 /* Product: '<S647>/u(1)*u(3)' */
12562 _rtB->kxj = _rtB->Sum_mx[0] * _rtB->Sum_mx[2];
12563
12564 /* Product: '<S647>/u(2)*u(4)*u(6)' */
12565 _rtB->ixk_a = _rtB->Sum_mx[1] * _rtB->p_p[0] * _rtB->p_p[2];
12566
12567 /* Sum: '<S647>/Sum' */
12568 _rtB->VectorConcatenate_mr[2] = _rtB->kxj + _rtB->ixk_a;
12569
12570 /* Product: '<S642>/u(3)*u(5)' */
12571 _rtB->VectorConcatenate_mr[3] = _rtB->Sum_mx[2] * _rtB->p_p[1];
12572
12573 /* Product: '<S645>/u(1)*u(2)*u(3)' */
12574 _rtB->kxj = _rtB->Sum_mx[0] * _rtB->Sum_mx[1] * _rtB->Sum_mx[2];
12575
12576 /* Product: '<S645>/u(4)*u(6)' */
12577 _rtB->ixk_a = _rtB->p_p[0] * _rtB->p_p[2];
12578
12579 /* Sum: '<S645>/Sum' */
12580 _rtB->VectorConcatenate_mr[4] = _rtB->kxj + _rtB->ixk_a;
12581
12582 /* Product: '<S648>/u(2)*u(3)*u(4)' */
12583 _rtB->kxj = _rtB->Sum_mx[1] * _rtB->Sum_mx[2] * _rtB->p_p[0];
12584
12585 /* Product: '<S648>/u(1)*u(6)' */
12586 _rtB->ixk_a = _rtB->p_p[2] * _rtB->Sum_mx[0];
12587
12588 /* Sum: '<S648>/Sum' */
12589 _rtB->VectorConcatenate_mr[5] = _rtB->kxj - _rtB->ixk_a;
12590
12591 /* Gain: '<S643>/Gain2' */
12592 _rtB->VectorConcatenate_mr[6] = -_rtB->Sum_mx[1];
12593
12594 /* Product: '<S646>/u(1)*u(3)' */
12595 _rtB->VectorConcatenate_mr[7] = _rtB->Sum_mx[0] * _rtB->p_p[1];
12596
12597 /* Product: '<S649>/u(4)*u(5)' */
12598 _rtB->VectorConcatenate_mr[8] = _rtB->p_p[0] * _rtB->p_p[1];
12599
12600 /* Product: '<S635>/j x k' */
12601 _rtB->kxj = _rtB->Sum1_d[1] * AFCS_MODEL1_rtC(S)->LeftMain_c[2];
12602
12603 /* Product: '<S635>/k x i' */
12604 _rtB->ixk_a = _rtB->Sum1_d[2] * AFCS_MODEL1_rtC(S)->LeftMain_c[0];
12605
12606 /* Product: '<S635>/i x j' */
12607 _rtB->jxi_p = _rtB->Sum1_d[0] * AFCS_MODEL1_rtC(S)->LeftMain_c[1];
12608
12609 /* Product: '<S636>/k x j' */
12610 _rtB->ixk = _rtB->Sum1_d[2] * AFCS_MODEL1_rtC(S)->LeftMain_c[1];
12611
12612 /* Product: '<S636>/i x k' */
12613 _rtB->jxi_k = _rtB->Sum1_d[0] * AFCS_MODEL1_rtC(S)->LeftMain_c[2];
12614
12615 /* Product: '<S636>/j x i' */
12616 _rtB->jxi = _rtB->Sum1_d[1] * AFCS_MODEL1_rtC(S)->LeftMain_c[0];
12617
12618 /* Sum: '<S622>/Sum' */
12619 _rtB->Sum_mx[0] = _rtB->kxj - _rtB->ixk;
12620 _rtB->Sum_mx[1] = _rtB->ixk_a - _rtB->jxi_k;
12621 _rtB->Sum_mx[2] = _rtB->jxi_p - _rtB->jxi;
12622 for (i_1 = 0; i_1 < 3; i_1++) {
12623 /* Math: '<S617>/Math Function1' */
12624 _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_mr[i_1];
12625 _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_mr[i_1 + 3];
12626 _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_mr[i_1 + 6];
12627
12628 /* Sum: '<S617>/Sum1' */
12629 _rtB->Sum_mx[i_1] += _rtB->UnitConversion_o[i_1];
12630 }
12631
12632 /* Product: '<S617>/Product2' */
12633 for (i_1 = 0; i_1 < 3; i_1++) {
12634 _rtB->p_p[i_1] = 0.0;
12635 _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1] * _rtB->Sum_mx[0];
12636 _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1 + 3] * _rtB->Sum_mx[1];
12637 _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1 + 6] * _rtB->Sum_mx[2];
12638 }
12639
12640 /* End of Product: '<S617>/Product2' */
12641
12642 /* Product: '<S632>/Product' */
12643 _rtB->kxj = _rtB->p_p[0] * _rtB->p_p[0];
12644
12645 /* Product: '<S632>/Product1' */
12646 _rtB->ixk_a = _rtB->p_p[1] * _rtB->p_p[1];
12647
12648 /* Product: '<S632>/Product2' */
12649 _rtB->jxi_p = _rtB->p_p[2] * _rtB->p_p[2];
12650
12651 /* Sum: '<S632>/Sum' */
12652 _rtB->kxj = (_rtB->kxj + _rtB->ixk_a) + _rtB->jxi_p;
12653
12654 /* Math: '<S617>/Airspeed'
12655 *
12656 * About '<S617>/Airspeed':
12657 * Operator: sqrt
12658 */
12659 if (_rtB->kxj < 0.0) {
12660 _rtB->kxj = -sqrt(fabs(_rtB->kxj));
12661 } else {
12662 _rtB->kxj = sqrt(_rtB->kxj);
12663 }
12664
12665 /* End of Math: '<S617>/Airspeed' */
12666
12667 /* Saturate: '<S617>/Saturation' */
12668 if (_rtB->kxj <= 0.01) {
12669 _rtB->kxj = 0.01;
12670 }
12671
12672 /* End of Saturate: '<S617>/Saturation' */
12673
12674 /* Product: '<S617>/Divide' */
12675 _rtB->kxj = _rtB->p_p[1] / _rtB->kxj;
12676
12677 /* Trigonometry: '<S617>/Trigonometric Function' */
12678 if (_rtB->kxj > 1.0) {
12679 TrigonometricFunction4_o = 1.0;
12680 } else if (_rtB->kxj < -1.0) {
12681 TrigonometricFunction4_o = -1.0;
12682 } else {
12683 TrigonometricFunction4_o = _rtB->kxj;
12684 }
12685
12686 _rtB->kxj = asin(TrigonometricFunction4_o);
12687
12688 /* End of Trigonometry: '<S617>/Trigonometric Function' */
12689
12690 /* Gain: '<S625>/Unit Conversion' */
12691 _rtB->UnitConversion_h = 57.295779513082323 * _rtB->kxj;
12692
12693 /* Abs: '<S617>/Abs1' */
12694 _rtB->kxj = fabs(_rtB->UnitConversion_h);
12695
12696 /* Saturate: '<S617>/Saturation7' */
12697 if (_rtB->kxj > 10.0) {
12698 _rtB->kxj = 10.0;
12699 } else {
12700 if (_rtB->kxj < 0.0) {
12701 _rtB->kxj = 0.0;
12702 }
12703 }
12704
12705 /* End of Saturate: '<S617>/Saturation7' */
12706
12707 /* Fcn: '<S617>/Fcn1' */
12708 _rtB->kxj = 0.08 * _rtB->kxj - 0.003 * AFCS_MODEL1_rt_powd_snf(_rtB->kxj,
12709 2.0);
12710
12711 /* Product: '<S617>/Product18' */
12712 _rtB->Product18 = _rtB->kxj * _rtB->ixj;
12713
12714 /* RelationalOperator: '<S630>/LowerRelop1' */
12715 _rtB->LowerRelop1 = (_rtB->Product18 > _rtB->kxi);
12716
12717 /* Switch: '<S630>/Switch2' */
12718 if (!_rtB->LowerRelop1) {
12719 /* RelationalOperator: '<S630>/UpperRelop' incorporates:
12720 * Constant: '<S617>/Constant11'
12721 */
12722 _rtB->UpperRelop_k = (_rtB->Product18 < 0.0);
12723
12724 /* Switch: '<S630>/Switch' incorporates:
12725 * Constant: '<S617>/Constant11'
12726 */
12727 if (_rtB->UpperRelop_k) {
12728 _rtB->Switch_a = 0.0;
12729 } else {
12730 _rtB->Switch_a = _rtB->Product18;
12731 }
12732
12733 /* End of Switch: '<S630>/Switch' */
12734 _rtB->kxi = _rtB->Switch_a;
12735 }
12736
12737 /* End of Switch: '<S630>/Switch2' */
12738
12739 /* Signum: '<S617>/Sign1' */
12740 if (_rtB->UnitConversion_h < 0.0) {
12741 _rtB->kxj = -1.0;
12742 } else if (_rtB->UnitConversion_h > 0.0) {
12743 _rtB->kxj = 1.0;
12744 } else if (_rtB->UnitConversion_h == 0.0) {
12745 _rtB->kxj = 0.0;
12746 } else {
12747 _rtB->kxj = _rtB->UnitConversion_h;
12748 }
12749
12750 /* End of Signum: '<S617>/Sign1' */
12751
12752 /* Product: '<S617>/Product19' */
12753 _rtB->Product19 = _rtB->kxi * _rtB->kxj;
12754
12755 /* Sum: '<S617>/Sum4' */
12756 _rtB->kxi = _rtB->Product19;
12757
12758 /* Gain: '<S617>/Gain2' */
12759 _rtB->Gain2 = _rtB->kxi;
12760 }
12761
12762 if (ssIsSampleHit(S, 1, 0)) {
12763 /* Gain: '<S466>/zground' */
12764 _rtB->zground = -_rtB->DataTypeConversion1;
12765 }
12766
12767 /* Outputs for Atomic SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */
12768 AFCS_MODEL1_LMainGearGeometryForcesandMoments(S, _rtB->zground, _rtB->Alt,
12769 _rtB->Merge_n[2], _rtB->Merge_n[1], AFCS_MODEL1_rtC(S)->LeftMain,
12770 _rtB->Saturation4, _rtB->Gain2, &_rtB->LMainGearGeometryForcesandMomen,
12771 (ConstB_LMainGearGeometryForce_T *)&AFCS_MODEL1_rtC(S)
12772 ->LMainGearGeometryForcesandMomen);
12773
12774 /* End of Outputs for SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */
12775 if (1) {
12776 /* Product: '<S617>/Product15' */
12777 _rtB->kxi = 0.0 * _rtB->jxk;
12778
12779 /* Sum: '<S617>/Add1' incorporates:
12780 * Constant: '<S617>/Constant2'
12781 */
12782 _rtB->kxi += 0.02;
12783
12784 /* Product: '<S617>/Product6' */
12785 _rtB->kxi *= _rtB->ixj;
12786
12787 /* Saturate: '<S617>/Saturation5' */
12788 if (_rtB->kxi > 0.7) {
12789 _rtB->Saturation5 = 0.7;
12790 } else if (_rtB->kxi < 0.0) {
12791 _rtB->Saturation5 = 0.0;
12792 } else {
12793 _rtB->Saturation5 = _rtB->kxi;
12794 }
12795
12796 /* End of Saturate: '<S617>/Saturation5' */
12797
12798 /* Math: '<S617>/Math Function5' */
12799 _rtB->jxk = _rtB->Saturation5 * _rtB->Saturation5;
12800
12801 /* Sum: '<S617>/Subtract' */
12802 _rtB->jxk = AFCS_MODEL1_rtC(S)->MathFunction4 - _rtB->jxk;
12803
12804 /* Sqrt: '<S617>/Sqrt' */
12805 _rtB->jxk = sqrt(_rtB->jxk);
12806
12807 /* Product: '<S639>/j x k' */
12808 _rtB->kxi = _rtB->Sum1_d[1] * AFCS_MODEL1_rtC(S)->RightMain_k[2];
12809
12810 /* Product: '<S639>/k x i' */
12811 _rtB->kxj = _rtB->Sum1_d[2] * AFCS_MODEL1_rtC(S)->RightMain_k[0];
12812
12813 /* Product: '<S639>/i x j' */
12814 _rtB->ixk_a = _rtB->Sum1_d[0] * AFCS_MODEL1_rtC(S)->RightMain_k[1];
12815
12816 /* Product: '<S640>/k x j' */
12817 _rtB->jxi_p = _rtB->Sum1_d[2] * AFCS_MODEL1_rtC(S)->RightMain_k[1];
12818
12819 /* Product: '<S640>/i x k' */
12820 _rtB->ixk = _rtB->Sum1_d[0] * AFCS_MODEL1_rtC(S)->RightMain_k[2];
12821
12822 /* Product: '<S640>/j x i' */
12823 _rtB->jxi_k = _rtB->Sum1_d[1] * AFCS_MODEL1_rtC(S)->RightMain_k[0];
12824
12825 /* Sum: '<S624>/Sum' */
12826 _rtB->Sum_mx[0] = _rtB->kxi - _rtB->jxi_p;
12827 _rtB->Sum_mx[1] = _rtB->kxj - _rtB->ixk;
12828 _rtB->Sum_mx[2] = _rtB->ixk_a - _rtB->jxi_k;
12829 for (i_1 = 0; i_1 < 3; i_1++) {
12830 /* Math: '<S617>/Math Function2' */
12831 _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_mr[i_1];
12832 _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_mr[i_1 + 3];
12833 _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_mr[i_1 + 6];
12834
12835 /* Sum: '<S617>/Sum2' */
12836 _rtB->Sum_mx[i_1] += _rtB->UnitConversion_o[i_1];
12837 }
12838
12839 /* Product: '<S617>/Product4' */
12840 for (i_1 = 0; i_1 < 3; i_1++) {
12841 _rtB->p_p[i_1] = 0.0;
12842 _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1] * _rtB->Sum_mx[0];
12843 _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1 + 3] * _rtB->Sum_mx[1];
12844 _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1 + 6] * _rtB->Sum_mx[2];
12845 }
12846
12847 /* End of Product: '<S617>/Product4' */
12848
12849 /* Product: '<S633>/Product' */
12850 _rtB->kxi = _rtB->p_p[0] * _rtB->p_p[0];
12851
12852 /* Product: '<S633>/Product1' */
12853 _rtB->kxj = _rtB->p_p[1] * _rtB->p_p[1];
12854
12855 /* Product: '<S633>/Product2' */
12856 _rtB->ixk_a = _rtB->p_p[2] * _rtB->p_p[2];
12857
12858 /* Sum: '<S633>/Sum' */
12859 _rtB->kxi = (_rtB->kxi + _rtB->kxj) + _rtB->ixk_a;
12860
12861 /* Math: '<S617>/Airspeed1'
12862 *
12863 * About '<S617>/Airspeed1':
12864 * Operator: sqrt
12865 */
12866 if (_rtB->kxi < 0.0) {
12867 _rtB->kxi = -sqrt(fabs(_rtB->kxi));
12868 } else {
12869 _rtB->kxi = sqrt(_rtB->kxi);
12870 }
12871
12872 /* End of Math: '<S617>/Airspeed1' */
12873
12874 /* Saturate: '<S617>/Saturation1' */
12875 if (_rtB->kxi <= 0.01) {
12876 _rtB->kxi = 0.01;
12877 }
12878
12879 /* End of Saturate: '<S617>/Saturation1' */
12880
12881 /* Product: '<S617>/Divide1' */
12882 _rtB->kxi = _rtB->p_p[1] / _rtB->kxi;
12883
12884 /* Trigonometry: '<S617>/Trigonometric Function1' */
12885 if (_rtB->kxi > 1.0) {
12886 TrigonometricFunction4_o = 1.0;
12887 } else if (_rtB->kxi < -1.0) {
12888 TrigonometricFunction4_o = -1.0;
12889 } else {
12890 TrigonometricFunction4_o = _rtB->kxi;
12891 }
12892
12893 _rtB->kxi = asin(TrigonometricFunction4_o);
12894
12895 /* End of Trigonometry: '<S617>/Trigonometric Function1' */
12896
12897 /* Gain: '<S626>/Unit Conversion' */
12898 _rtB->UnitConversion_c = 57.295779513082323 * _rtB->kxi;
12899
12900 /* Abs: '<S617>/Abs2' */
12901 _rtB->kxi = fabs(_rtB->UnitConversion_c);
12902
12903 /* Saturate: '<S617>/Saturation8' */
12904 if (_rtB->kxi > 10.0) {
12905 _rtB->kxi = 10.0;
12906 } else {
12907 if (_rtB->kxi < 0.0) {
12908 _rtB->kxi = 0.0;
12909 }
12910 }
12911
12912 /* End of Saturate: '<S617>/Saturation8' */
12913
12914 /* Fcn: '<S617>/Fcn2' */
12915 _rtB->kxi = 0.08 * _rtB->kxi - 0.003 * AFCS_MODEL1_rt_powd_snf(_rtB->kxi,
12916 2.0);
12917
12918 /* Product: '<S617>/Product20' */
12919 _rtB->Product20 = _rtB->kxi * _rtB->ixj;
12920
12921 /* RelationalOperator: '<S629>/LowerRelop1' */
12922 _rtB->LowerRelop1 = (_rtB->Product20 > _rtB->jxk);
12923
12924 /* Switch: '<S629>/Switch2' */
12925 if (!_rtB->LowerRelop1) {
12926 /* RelationalOperator: '<S629>/UpperRelop' incorporates:
12927 * Constant: '<S617>/Constant7'
12928 */
12929 _rtB->UpperRelop_p = (_rtB->Product20 < 0.0);
12930
12931 /* Switch: '<S629>/Switch' incorporates:
12932 * Constant: '<S617>/Constant7'
12933 */
12934 if (_rtB->UpperRelop_p) {
12935 _rtB->Switch_g = 0.0;
12936 } else {
12937 _rtB->Switch_g = _rtB->Product20;
12938 }
12939
12940 /* End of Switch: '<S629>/Switch' */
12941 _rtB->jxk = _rtB->Switch_g;
12942 }
12943
12944 /* End of Switch: '<S629>/Switch2' */
12945
12946 /* Signum: '<S617>/Sign2' */
12947 if (_rtB->UnitConversion_c < 0.0) {
12948 _rtB->kxi = -1.0;
12949 } else if (_rtB->UnitConversion_c > 0.0) {
12950 _rtB->kxi = 1.0;
12951 } else if (_rtB->UnitConversion_c == 0.0) {
12952 _rtB->kxi = 0.0;
12953 } else {
12954 _rtB->kxi = _rtB->UnitConversion_c;
12955 }
12956
12957 /* End of Signum: '<S617>/Sign2' */
12958
12959 /* Product: '<S617>/Product21' */
12960 _rtB->Product21 = _rtB->jxk * _rtB->kxi;
12961
12962 /* Sum: '<S617>/Sum5' */
12963 _rtB->jxk = _rtB->Product21;
12964
12965 /* Gain: '<S617>/Gain3' */
12966 _rtB->Gain3 = _rtB->jxk;
12967 }
12968
12969 /* Outputs for Atomic SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */
12970 AFCS_MODEL1_RMainGearGeometryForcesandMoments(S, _rtB->zground, _rtB->Alt,
12971 _rtB->Merge_n[2], _rtB->Merge_n[1], AFCS_MODEL1_rtC(S)->RightMain,
12972 _rtB->Saturation5, _rtB->Gain3, _rtB->Merge_n,
12973 &_rtB->RMainGearGeometryForcesandMomen, (ConstB_RMainGearGeometryForce_T *)
12974 &AFCS_MODEL1_rtC(S)->RMainGearGeometryForcesandMomen);
12975
12976 /* End of Outputs for SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */
12977 if (ssIsSampleHit(S, 1, 0)) {
12978 /* UnitDelay: '<S682>/Unit Delay' */
12979 _rtB->Sum1_fe = ((int8_T *)ssGetDWork(S, 371))[0];
12980
12981 /* Sum: '<S682>/Sum' incorporates:
12982 * Constant: '<S446>/Constant2'
12983 */
12984 _rtB->Sum1_fe = (int8_T)(1 - _rtB->Sum1_fe);
12985
12986 /* RelationalOperator: '<S684>/Compare' incorporates:
12987 * Constant: '<S684>/Constant'
12988 */
12989 _rtB->Compare = (uint8_T)(_rtB->Sum1_fe > 0);
12990
12991 /* Logic: '<S682>/Logical Operator' */
12992 _rtB->LogicalOperator_m = (_rtB->Compare != 0);
12993
12994 /* UnitDelay: '<S683>/Unit Delay' */
12995 _rtB->Sum1_fe = ((int8_T *)ssGetDWork(S, 372))[0];
12996
12997 /* Sum: '<S683>/Sum1' incorporates:
12998 * Constant: '<S446>/Constant2'
12999 */
13000 _rtB->Sum1_fe = (int8_T)(1 - _rtB->Sum1_fe);
13001
13002 /* RelationalOperator: '<S685>/Compare' incorporates:
13003 * Constant: '<S685>/Constant'
13004 */
13005 _rtB->Compare_i = (_rtB->Sum1_fe < 0);
13006
13007 /* UnitDelay: '<S619>/Unit Delay1' */
13008 _rtB->UnitDelay1 = ((real_T *)ssGetDWork(S, 0))[0];
13009 }
13010
13011 if (1) {
13012 /* Product: '<S617>/Product3' incorporates:
13013 * Constant: '<S617>/Constant'
13014 */
13015 _rtB->jxk = 0.02 * _rtB->ixj;
13016
13017 /* Saturate: '<S617>/Saturation3' */
13018 if (_rtB->jxk > 0.7) {
13019 _rtB->Saturation3 = 0.7;
13020 } else if (_rtB->jxk < 0.0) {
13021 _rtB->Saturation3 = 0.0;
13022 } else {
13023 _rtB->Saturation3 = _rtB->jxk;
13024 }
13025
13026 /* End of Saturate: '<S617>/Saturation3' */
13027
13028 /* Math: '<S617>/Math Function9' */
13029 _rtB->jxk = _rtB->Saturation3 * _rtB->Saturation3;
13030
13031 /* Sum: '<S617>/Subtract2' */
13032 _rtB->jxk = AFCS_MODEL1_rtC(S)->MathFunction8 - _rtB->jxk;
13033
13034 /* Product: '<S637>/j x k' */
13035 _rtB->kxi = _rtB->Sum1_d[1] * AFCS_MODEL1_rtC(S)->Nose_d[2];
13036
13037 /* Product: '<S637>/k x i' */
13038 _rtB->kxj = _rtB->Sum1_d[2] * AFCS_MODEL1_rtC(S)->Nose_d[0];
13039
13040 /* Product: '<S637>/i x j' */
13041 _rtB->ixk_a = _rtB->Sum1_d[0] * AFCS_MODEL1_rtC(S)->Nose_d[1];
13042
13043 /* Product: '<S638>/k x j' */
13044 _rtB->jxi_p = _rtB->Sum1_d[2] * AFCS_MODEL1_rtC(S)->Nose_d[1];
13045
13046 /* Product: '<S638>/i x k' */
13047 _rtB->ixk = _rtB->Sum1_d[0] * AFCS_MODEL1_rtC(S)->Nose_d[2];
13048
13049 /* Product: '<S638>/j x i' */
13050 _rtB->jxi_k = _rtB->Sum1_d[1] * AFCS_MODEL1_rtC(S)->Nose_d[0];
13051
13052 /* Sum: '<S623>/Sum' */
13053 _rtB->Sum_mx[0] = _rtB->kxi - _rtB->jxi_p;
13054 _rtB->Sum_mx[1] = _rtB->kxj - _rtB->ixk;
13055 _rtB->Sum_mx[2] = _rtB->ixk_a - _rtB->jxi_k;
13056 for (i_1 = 0; i_1 < 3; i_1++) {
13057 /* Math: '<S617>/Math Function3' */
13058 _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_mr[i_1];
13059 _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_mr[i_1 + 3];
13060 _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_mr[i_1 + 6];
13061
13062 /* Sum: '<S617>/Sum' */
13063 _rtB->Sum_mx[i_1] += _rtB->UnitConversion_o[i_1];
13064 }
13065
13066 /* Product: '<S617>/Product1' */
13067 for (i_1 = 0; i_1 < 3; i_1++) {
13068 _rtB->p_p[i_1] = 0.0;
13069 _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1] * _rtB->Sum_mx[0];
13070 _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1 + 3] * _rtB->Sum_mx[1];
13071 _rtB->p_p[i_1] += _rtB->MathFunction1_a[i_1 + 6] * _rtB->Sum_mx[2];
13072 }
13073
13074 /* End of Product: '<S617>/Product1' */
13075
13076 /* Product: '<S634>/Product' */
13077 _rtB->kxi = _rtB->p_p[0] * _rtB->p_p[0];
13078
13079 /* Product: '<S634>/Product1' */
13080 _rtB->kxj = _rtB->p_p[1] * _rtB->p_p[1];
13081
13082 /* Product: '<S634>/Product2' */
13083 _rtB->ixk_a = _rtB->p_p[2] * _rtB->p_p[2];
13084
13085 /* Sum: '<S634>/Sum' */
13086 _rtB->kxi = (_rtB->kxi + _rtB->kxj) + _rtB->ixk_a;
13087
13088 /* Math: '<S617>/Airspeed2'
13089 *
13090 * About '<S617>/Airspeed2':
13091 * Operator: sqrt
13092 */
13093 if (_rtB->kxi < 0.0) {
13094 _rtB->kxi = -sqrt(fabs(_rtB->kxi));
13095 } else {
13096 _rtB->kxi = sqrt(_rtB->kxi);
13097 }
13098
13099 /* End of Math: '<S617>/Airspeed2' */
13100
13101 /* Saturate: '<S617>/Saturation2' */
13102 if (_rtB->kxi <= 0.01) {
13103 _rtB->kxi = 0.01;
13104 }
13105
13106 /* End of Saturate: '<S617>/Saturation2' */
13107
13108 /* Product: '<S617>/Divide2' */
13109 _rtB->kxi = _rtB->p_p[1] / _rtB->kxi;
13110
13111 /* Trigonometry: '<S617>/Trigonometric Function2' */
13112 if (_rtB->kxi > 1.0) {
13113 TrigonometricFunction4_o = 1.0;
13114 } else if (_rtB->kxi < -1.0) {
13115 TrigonometricFunction4_o = -1.0;
13116 } else {
13117 TrigonometricFunction4_o = _rtB->kxi;
13118 }
13119
13120 _rtB->kxi = asin(TrigonometricFunction4_o);
13121
13122 /* End of Trigonometry: '<S617>/Trigonometric Function2' */
13123
13124 /* Gain: '<S627>/Unit Conversion' */
13125 _rtB->UnitConversion_oi = 57.295779513082323 * _rtB->kxi;
13126
13127 /* Integrator: '<S683>/Integrator' */
13128 /* Limited Integrator */
13129 if (ssIsMajorTimeStep(S)) {
13130 resetIntg = false;
13131 didZcEventOccur = (_rtB->Compare_i && (_rtZCE->Integrator_Reset_ZCE !=
13132 POS_ZCSIG));
13133 _rtZCE->Integrator_Reset_ZCE = _rtB->Compare_i;
13134
13135 /* evaluate zero-crossings */
13136 if (didZcEventOccur || (((int_T *)ssGetDWork(S, 348))[0] != 0)) {
13137 resetIntg = true;
13138 _rtX->Integrator_CSTATE = _rtB->UnitDelay1;
13139 }
13140
13141 if (resetIntg) {
13142 ssSetSolverNeedsReset(S);
13143 }
13144 }
13145
13146 if (_rtX->Integrator_CSTATE >= 8.0) {
13147 if (_rtX->Integrator_CSTATE != 8.0) {
13148 _rtX->Integrator_CSTATE = 8.0;
13149 ssSetSolverNeedsReset(S);
13150 }
13151 } else {
13152 if ((_rtX->Integrator_CSTATE <= -8.0) && (_rtX->Integrator_CSTATE !=
13153 -8.0)) {
13154 _rtX->Integrator_CSTATE = -8.0;
13155 ssSetSolverNeedsReset(S);
13156 }
13157 }
13158
13159 _rtB->Integrator = _rtX->Integrator_CSTATE;
13160
13161 /* End of Integrator: '<S683>/Integrator' */
13162
13163 /* Integrator: '<S682>/Integrator' */
13164 /* Limited Integrator */
13165 if (ssIsMajorTimeStep(S)) {
13166 resetIntg = false;
13167 didZcEventOccur = (_rtB->LogicalOperator_m &&
13168 (_rtZCE->Integrator_Reset_ZCE_f != POS_ZCSIG));
13169 _rtZCE->Integrator_Reset_ZCE_f = _rtB->LogicalOperator_m;
13170
13171 /* evaluate zero-crossings */
13172 if (didZcEventOccur || (((int_T *)ssGetDWork(S, 349))[0] != 0)) {
13173 resetIntg = true;
13174 _rtX->Integrator_CSTATE_l = _rtB->Integrator;
13175 }
13176
13177 if (resetIntg) {
13178 ssSetSolverNeedsReset(S);
13179 }
13180 }
13181
13182 if (_rtX->Integrator_CSTATE_l >= 30.0) {
13183 if (_rtX->Integrator_CSTATE_l != 30.0) {
13184 _rtX->Integrator_CSTATE_l = 30.0;
13185 ssSetSolverNeedsReset(S);
13186 }
13187 } else {
13188 if ((_rtX->Integrator_CSTATE_l <= -30.0) && (_rtX->Integrator_CSTATE_l
13189 != -30.0)) {
13190 _rtX->Integrator_CSTATE_l = -30.0;
13191 ssSetSolverNeedsReset(S);
13192 }
13193 }
13194
13195 _rtB->Integrator_m = _rtX->Integrator_CSTATE_l;
13196
13197 /* End of Integrator: '<S682>/Integrator' */
13198
13199 /* Sum: '<S617>/Sum3' */
13200 _rtB->Sum3 = _rtB->UnitConversion_oi - _rtB->Integrator_m;
13201
13202 /* Abs: '<S617>/Abs' */
13203 _rtB->kxi = fabs(_rtB->Sum3);
13204
13205 /* Saturate: '<S617>/Saturation6' */
13206 if (_rtB->kxi > 10.0) {
13207 _rtB->kxi = 10.0;
13208 } else {
13209 if (_rtB->kxi < 0.0) {
13210 _rtB->kxi = 0.0;
13211 }
13212 }
13213
13214 /* End of Saturate: '<S617>/Saturation6' */
13215
13216 /* Fcn: '<S617>/Fcn' */
13217 _rtB->kxi = 0.08 * _rtB->kxi - 0.003 * AFCS_MODEL1_rt_powd_snf(_rtB->kxi,
13218 2.0);
13219
13220 /* Product: '<S617>/Product16' */
13221 _rtB->Product16 = _rtB->kxi * _rtB->ixj;
13222
13223 /* RelationalOperator: '<S631>/LowerRelop1' */
13224 _rtB->LowerRelop1 = (_rtB->Product16 > _rtB->jxk);
13225
13226 /* Switch: '<S631>/Switch2' */
13227 if (!_rtB->LowerRelop1) {
13228 /* RelationalOperator: '<S631>/UpperRelop' incorporates:
13229 * Constant: '<S617>/Constant12'
13230 */
13231 _rtB->UpperRelop = (_rtB->Product16 < 0.0);
13232
13233 /* Switch: '<S631>/Switch' incorporates:
13234 * Constant: '<S617>/Constant12'
13235 */
13236 if (_rtB->UpperRelop) {
13237 _rtB->Switch_o = 0.0;
13238 } else {
13239 _rtB->Switch_o = _rtB->Product16;
13240 }
13241
13242 /* End of Switch: '<S631>/Switch' */
13243 _rtB->jxk = _rtB->Switch_o;
13244 }
13245
13246 /* End of Switch: '<S631>/Switch2' */
13247
13248 /* Signum: '<S617>/Sign' */
13249 if (_rtB->Sum3 < 0.0) {
13250 _rtB->kxi = -1.0;
13251 } else if (_rtB->Sum3 > 0.0) {
13252 _rtB->kxi = 1.0;
13253 } else if (_rtB->Sum3 == 0.0) {
13254 _rtB->kxi = 0.0;
13255 } else {
13256 _rtB->kxi = _rtB->Sum3;
13257 }
13258
13259 /* End of Signum: '<S617>/Sign' */
13260
13261 /* Product: '<S617>/Product17' */
13262 _rtB->jxk *= _rtB->kxi;
13263
13264 /* Gain: '<S617>/Gain1' */
13265 _rtB->Gain1 = _rtB->jxk;
13266 }
13267
13268 /* Outputs for Atomic SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */
13269 AFCS_MODEL1_NoseGearGeometryForcesandMoments(S, _rtB->zground, _rtB->Alt,
13270 _rtB->Merge_n[2], _rtB->Merge_n[1], AFCS_MODEL1_rtC(S)->Nose,
13271 _rtB->Saturation3, _rtB->Gain1, _rtB->Merge_n,
13272 &_rtB->NoseGearGeometryForcesandMoment, (ConstB_NoseGearGeometryForces_T *)
13273 &AFCS_MODEL1_rtC(S)->NoseGearGeometryForcesandMoment);
13274
13275 /* End of Outputs for SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */
13276 if (1) {
13277 /* Sum: '<S466>/Sum2' */
13278 _rtB->Sum2_g[0] =
13279 (_rtB->LMainGearGeometryForcesandMomen.LocalForcestoBodyAxes1[0] +
13280 _rtB->RMainGearGeometryForcesandMomen.LocalForcestoBodyAxes1[0]) +
13281 _rtB->NoseGearGeometryForcesandMoment.LocalForcestoBodyAxes1[0];
13282 _rtB->Sum2_g[3] = (_rtB->LMainGearGeometryForcesandMomen.Gain2[0] +
13283 _rtB->RMainGearGeometryForcesandMomen.Gain2[0]) +
13284 _rtB->NoseGearGeometryForcesandMoment.Gain1[0];
13285
13286 /* Sum: '<S446>/Sum1' */
13287 tmp_0[0] = _rtB->Product2_f[0];
13288 tmp_0[3] = _rtB->Sum1[0];
13289
13290 /* Sum: '<S466>/Sum2' */
13291 _rtB->Sum2_g[1] =
13292 (_rtB->LMainGearGeometryForcesandMomen.LocalForcestoBodyAxes1[1] +
13293 _rtB->RMainGearGeometryForcesandMomen.LocalForcestoBodyAxes1[1]) +
13294 _rtB->NoseGearGeometryForcesandMoment.LocalForcestoBodyAxes1[1];
13295 _rtB->Sum2_g[4] = (_rtB->LMainGearGeometryForcesandMomen.Gain2[1] +
13296 _rtB->RMainGearGeometryForcesandMomen.Gain2[1]) +
13297 _rtB->NoseGearGeometryForcesandMoment.Gain1[1];
13298
13299 /* Sum: '<S446>/Sum1' */
13300 tmp_0[1] = _rtB->Product2_f[1];
13301 tmp_0[4] = _rtB->Sum1[1];
13302
13303 /* Sum: '<S466>/Sum2' */
13304 _rtB->Sum2_g[2] =
13305 (_rtB->LMainGearGeometryForcesandMomen.LocalForcestoBodyAxes1[2] +
13306 _rtB->RMainGearGeometryForcesandMomen.LocalForcestoBodyAxes1[2]) +
13307 _rtB->NoseGearGeometryForcesandMoment.LocalForcestoBodyAxes1[2];
13308 _rtB->Sum2_g[5] = (_rtB->LMainGearGeometryForcesandMomen.Gain2[2] +
13309 _rtB->RMainGearGeometryForcesandMomen.Gain2[2]) +
13310 _rtB->NoseGearGeometryForcesandMoment.Gain1[2];
13311
13312 /* Sum: '<S446>/Sum1' */
13313 tmp_0[2] = _rtB->Product2_f[2];
13314 tmp_0[5] = _rtB->Sum1[2];
13315 for (i_1 = 0; i_1 < 6; i_1++) {
13316 _rtB->Sum1_h[i_1] = (tmp_0[i_1] + _rtB->Sum1_h[i_1]) + _rtB->Sum2_g[i_1];
13317 }
13318
13319 /* Sum: '<S446>/Sum' */
13320 _rtB->Product_d[0] += _rtB->Sum1_h[0];
13321
13322 /* Product: '<S470>/Product' */
13323 _rtB->Product_d[0] /= AFCS_MODEL1_rtC(S)->Product4_d;
13324
13325 /* Sum: '<S469>/Sum6' */
13326 _rtB->Product1_f[0] = _rtB->Product_d[0] - _rtB->Product1_f[0];
13327
13328 /* Sum: '<S446>/Sum' */
13329 _rtB->Product_d[1] += _rtB->Sum1_h[1];
13330
13331 /* Product: '<S470>/Product' */
13332 _rtB->Product_d[1] /= AFCS_MODEL1_rtC(S)->Product4_d;
13333
13334 /* Sum: '<S469>/Sum6' */
13335 _rtB->Product1_f[1] = _rtB->Product_d[1] - _rtB->Product1_f[1];
13336
13337 /* Sum: '<S446>/Sum' */
13338 _rtB->Product_d[2] += _rtB->Sum1_h[2];
13339
13340 /* Product: '<S470>/Product' */
13341 _rtB->Product_d[2] /= AFCS_MODEL1_rtC(S)->Product4_d;
13342
13343 /* Sum: '<S469>/Sum6' */
13344 _rtB->Product1_f[2] = _rtB->Product_d[2] - _rtB->Product1_f[2];
13345
13346 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
13347 _rtB->Abx = _rtB->Product1_f[0];
13348
13349 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
13350 _rtB->Aby = _rtB->Product1_f[1];
13351
13352 /* Gain: '<S469>/Gain1' */
13353 _rtB->jxk = -_rtB->Product1_f[2];
13354
13355 /* SignalConversion: '<S446>/BusConversion_InsertedFor_Sensor1_at_inport_0' */
13356 _rtB->Abz = _rtB->jxk;
13357
13358 /* Product: '<S613>/Product' */
13359 _rtB->jxk = _rtB->Sum_m[0] * _rtB->Sum_m[0];
13360
13361 /* Product: '<S613>/Product1' */
13362 _rtB->kxi = _rtB->Sum_m[1] * _rtB->Sum_m[1];
13363
13364 /* Product: '<S613>/Product2' */
13365 _rtB->ixj = _rtB->Sum_m[2] * _rtB->Sum_m[2];
13366
13367 /* Sum: '<S613>/Sum' */
13368 _rtB->jxk = (_rtB->jxk + _rtB->kxi) + _rtB->ixj;
13369
13370 /* Sqrt: '<S606>/vt' */
13371 _rtB->jxk = sqrt(_rtB->jxk);
13372
13373 /* Product: '<S606>/Product1' */
13374 _rtB->Product1 = _rtB->jxk / _rtB->sound;
13375
13376 /* MATLAB Function: '<S464>/Embedded MATLAB Function' */
13377 /* MATLAB Function 'AFCS_MODEL1/Subsystem5/Subsystem3/Devond/Embedded MATLAB Function': '<S603>:1' */
13378 /* This block supports an embeddable subset of the MATLAB language. */
13379 /* See the help menu for details. */
13380 /* '<S603>:1:4' */
13381 if (_rtB->Product1 < 1.0) {
13382 /* '<S603>:1:5' */
13383 /* '<S603>:1:6' */
13384 TrigonometricFunction4_o = AFCS_MODEL1_rt_powd_snf(_rtB->Product1 *
13385 _rtB->Product1 * 0.19999999999999996 + 1.0, 3.5);
13386 } else {
13387 /* '<S603>:1:8' */
13388 TrigonometricFunction4_o = AFCS_MODEL1_rt_powd_snf(_rtB->Product1 *
13389 _rtB->Product1 * 2.4 / 2.0, 3.5000000000000004) /
13390 AFCS_MODEL1_rt_powd_snf((_rtB->Product1 * _rtB->Product1 *
13391 1.1666666666666667 - 0.28571428571428564) + 1.0, 2.5000000000000004);
13392 }
13393
13394 /* '<S603>:1:10' */
13395 TrigonometricFunction4_o = (TrigonometricFunction4_o - 1.0) * _rtB->PS;
13396 if (_rtB->Product1 < 1.0) {
13397 /* '<S603>:1:12' */
13398 /* '<S603>:1:13' */
13399 _rtB->VC = sqrt((AFCS_MODEL1_rt_powd_snf(TrigonometricFunction4_o /
13400 101325.0 + 1.0, 0.2857142857142857) - 1.0) * 5.0) * 340.294 * 3.6;
13401 } else {
13402 /* '<S603>:1:15' */
13403 _rtB->VC = 0.0;
13404 }
13405
13406 _rtB->QC = TrigonometricFunction4_o;
13407
13408 /* End of MATLAB Function: '<S464>/Embedded MATLAB Function' */
13409 }
13410
13411 if (ssIsSampleHit(S, 1, 0)) {
13412 }
13413
13414 if (1) {
13415 /* Product: '<S617>/Product' */
13416 _rtB->Product = 0.0 * _rtB->Saturation3;
13417 }
13418
13419 if (ssIsSampleHit(S, 1, 0)) {
13420 }
13421
13422 if (1) {
13423 /* Product: '<S617>/Product7' */
13424 _rtB->Product7 = 0.0 * _rtB->Gain1;
13425 }
13426
13427 if (ssIsSampleHit(S, 1, 0)) {
13428 }
13429
13430 if (1) {
13431 /* Product: '<S617>/Product8' */
13432 _rtB->Product8 = 0.0 * _rtB->Saturation4;
13433 }
13434
13435 if (ssIsSampleHit(S, 1, 0)) {
13436 }
13437
13438 if (1) {
13439 /* Product: '<S466>/Product' */
13440 _rtB->Product_g = _rtB->UnitConversion_oi * (real_T)
13441 _rtB->NoseGearGeometryForcesandMoment.Switch;
13442 }
13443
13444 if (ssIsSampleHit(S, 1, 0)) {
13445 }
13446
13447 if (1) {
13448 /* Product: '<S682>/Product' */
13449 _rtB->jxk = _rtB->Backlash;
13450
13451 /* Sum: '<S682>/Add' */
13452 _rtB->jxk -= _rtB->UnitDelay1;
13453
13454 /* Gain: '<S682>/Gain3' */
13455 _rtB->jxk *= 15.0;
13456
13457 /* Saturate: '<S682>/Saturation' */
13458 if (_rtB->jxk > 50.0) {
13459 _rtB->Saturation = 50.0;
13460 } else if (_rtB->jxk < -50.0) {
13461 _rtB->Saturation = -50.0;
13462 } else {
13463 _rtB->Saturation = _rtB->jxk;
13464 }
13465
13466 /* End of Saturate: '<S682>/Saturation' */
13467 }
13468
13469 if (ssIsSampleHit(S, 1, 0)) {
13470 }
13471
13472 if (1) {
13473 /* Sum: '<S683>/Sum' */
13474 _rtB->jxk = _rtB->Product_g - _rtB->UnitDelay1;
13475
13476 /* Gain: '<S683>/Gain' */
13477 _rtB->jxk *= 3.0;
13478
13479 /* Saturate: '<S683>/Saturation1' */
13480 if (_rtB->jxk > 12.0) {
13481 _rtB->Saturation1 = 12.0;
13482 } else if (_rtB->jxk < -12.0) {
13483 _rtB->Saturation1 = -12.0;
13484 } else {
13485 _rtB->Saturation1 = _rtB->jxk;
13486 }
13487
13488 /* End of Saturate: '<S683>/Saturation1' */
13489
13490 /* TransferFcn: '<S683>/Transfer Fcn2' */
13491 _rtB->TransferFcn2 = 0.0;
13492 _rtB->TransferFcn2 += 0.0 * _rtX->TransferFcn2_CSTATE[0];
13493 _rtB->TransferFcn2 += 16.0 * _rtX->TransferFcn2_CSTATE[1];
13494
13495 /* MATLAB Function: '<S469>/MATLAB Function' incorporates:
13496 * Constant: '<S14>/Constant1'
13497 * Constant: '<S14>/Constant2'
13498 * Constant: '<S469>/Constant'
13499 */
13500 /* MATLAB Function 'AFCS_MODEL1/Subsystem5/Subsystem3/Sensor/MATLAB Function': '<S769>:1' */
13501 /* '<S769>:1:3' */
13502 /* '<S769>:1:4' */
13503 _rtB->Xnorth = (_rtB->Lat_p - 40.0) * 6.37E+6 * 0.017453292519943295;
13504
13505 /* '<S769>:1:5' */
13506 _rtB->Yeast = cos((_rtB->Lat_p + 40.0) * 0.017453292519943295 / 2.0) *
13507 6.37E+6 * (_rtB->Lon_j - 30.0) * 0.017453292519943295;
13508
13509 /* '<S769>:1:6' */
13510 _rtB->mh = _rtB->Alt - _rtB->DataTypeConversion1;
13511
13512 /* SignalConversion: '<S446>/OutportBufferForENT' */
13513 _rtB->OutportBufferForENT[0] = _rtB->Xnorth;
13514 _rtB->OutportBufferForENT[1] = _rtB->Yeast;
13515 _rtB->OutportBufferForENT[2] = _rtB->mh;
13516
13517 /* Integrator: '<S781>/Integrator' */
13518 _rtB->jxk = _rtX->Integrator_CSTATE_a;
13519
13520 /* Sum: '<S781>/Sum' */
13521 _rtB->jxk += AFCS_MODEL1_rtC(S)->UnitConversion_b;
13522
13523 /* Trigonometry: '<S775>/sincos' */
13524 _rtB->kxi = cos(_rtB->jxk);
13525 _rtB->jxk = sin(_rtB->jxk);
13526
13527 /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn1' */
13528 _rtB->VectorConcatenate_ak[0] = _rtB->kxi;
13529
13530 /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn2' */
13531 _rtB->VectorConcatenate_ak[1] = _rtB->jxk;
13532
13533 /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn3' incorporates:
13534 * Constant: '<S775>/Zero'
13535 */
13536 _rtB->VectorConcatenate_ak[2] = 0.0;
13537
13538 /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn5' */
13539 _rtB->VectorConcatenate_ak[4] = _rtB->kxi;
13540
13541 /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn6' incorporates:
13542 * Constant: '<S775>/Zero'
13543 */
13544 _rtB->VectorConcatenate_ak[5] = 0.0;
13545
13546 /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn7' incorporates:
13547 * Constant: '<S775>/Zero'
13548 */
13549 _rtB->VectorConcatenate_ak[6] = 0.0;
13550
13551 /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn8' incorporates:
13552 * Constant: '<S775>/Zero'
13553 */
13554 _rtB->VectorConcatenate_ak[7] = 0.0;
13555
13556 /* SignalConversion: '<S794>/ConcatBufferAtVector ConcatenateIn9' incorporates:
13557 * Constant: '<S775>/Zero1'
13558 */
13559 _rtB->VectorConcatenate_ak[8] = 1.0;
13560
13561 /* UnaryMinus: '<S775>/Unary Minus' */
13562 _rtB->VectorConcatenate_ak[3] = -_rtB->jxk;
13563
13564 /* Math: '<S470>/Math Function1' */
13565 for (i_1 = 0; i_1 < 3; i_1++) {
13566 _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_n[i_1];
13567 _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_n[i_1 + 3];
13568 _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_n[i_1 + 6];
13569 }
13570
13571 /* End of Math: '<S470>/Math Function1' */
13572 for (i_1 = 0; i_1 < 3; i_1++) {
13573 /* Product: '<S470>/Product1' */
13574 _rtB->Product1_o[i_1] = 0.0;
13575
13576 /* Product: '<S795>/Product' */
13577 _rtB->Sum_mx[i_1] = 0.0;
13578
13579 /* Product: '<S796>/Product' incorporates:
13580 * Constant: '<S776>/Constant'
13581 */
13582 _rtB->Product1_f[i_1] = 0.0;
13583
13584 /* Product: '<S470>/Product1' */
13585 _rtB->Product1_o[i_1] += _rtB->MathFunction1_a[i_1] *
13586 _rtB->UnitConversion_o[0];
13587
13588 /* Product: '<S795>/Product' */
13589 _rtB->Sum_mx[i_1] += AFCS_MODEL1_rtC(S)->VectorConcatenate[i_1] *
13590 _rtB->Sum2_h[0];
13591
13592 /* Product: '<S796>/Product' incorporates:
13593 * Constant: '<S776>/Constant'
13594 */
13595 _rtB->Product1_f[i_1] += 0.0 * _rtB->Sum2_h[0];
13596
13597 /* Product: '<S470>/Product1' */
13598 _rtB->Product1_o[i_1] += _rtB->MathFunction1_a[i_1 + 3] *
13599 _rtB->UnitConversion_o[1];
13600
13601 /* Product: '<S795>/Product' */
13602 _rtB->Sum_mx[i_1] += AFCS_MODEL1_rtC(S)->VectorConcatenate[i_1 + 3] *
13603 _rtB->Sum2_h[1];
13604
13605 /* Product: '<S796>/Product' incorporates:
13606 * Constant: '<S776>/Constant'
13607 */
13608 _rtB->Product1_f[i_1] += 0.0 * _rtB->Sum2_h[1];
13609
13610 /* Product: '<S470>/Product1' */
13611 _rtB->Product1_o[i_1] += _rtB->MathFunction1_a[i_1 + 6] *
13612 _rtB->UnitConversion_o[2];
13613
13614 /* Product: '<S795>/Product' */
13615 _rtB->Sum_mx[i_1] += AFCS_MODEL1_rtC(S)->VectorConcatenate[i_1 + 6] *
13616 _rtB->Sum2_h[2];
13617
13618 /* Product: '<S796>/Product' incorporates:
13619 * Constant: '<S776>/Constant'
13620 */
13621 _rtB->Product1_f[i_1] += 0.0 * _rtB->Sum2_h[2];
13622 }
13623
13624 /* Product: '<S798>/j x k' */
13625 _rtB->jxk = _rtB->Sum2_h[1] * _rtB->Sum_mx[2];
13626
13627 /* Product: '<S798>/k x i' */
13628 _rtB->kxi = _rtB->Sum2_h[2] * _rtB->Sum_mx[0];
13629
13630 /* Product: '<S798>/i x j' */
13631 _rtB->ixj = _rtB->Sum2_h[0] * _rtB->Sum_mx[1];
13632
13633 /* Product: '<S799>/k x j' */
13634 _rtB->kxj = _rtB->Sum2_h[2] * _rtB->Sum_mx[1];
13635
13636 /* Product: '<S799>/i x k' */
13637 _rtB->ixk_a = _rtB->Sum2_h[0] * _rtB->Sum_mx[2];
13638
13639 /* Product: '<S799>/j x i' */
13640 _rtB->jxi_p = _rtB->Sum2_h[1] * _rtB->Sum_mx[0];
13641
13642 /* Sum: '<S797>/Sum' */
13643 _rtB->Sum_mx[0] = _rtB->jxk - _rtB->kxj;
13644 _rtB->Sum_mx[1] = _rtB->kxi - _rtB->ixk_a;
13645 _rtB->Sum_mx[2] = _rtB->ixj - _rtB->jxi_p;
13646
13647 /* Sum: '<S777>/Sum2' */
13648 _rtB->Product1_f[0] = (_rtB->Sum1_h[3] - _rtB->Product1_f[0]) -
13649 _rtB->Sum_mx[0];
13650 _rtB->Product1_f[1] = (_rtB->Sum1_h[4] - _rtB->Product1_f[1]) -
13651 _rtB->Sum_mx[1];
13652 _rtB->Product1_f[2] = (_rtB->Sum1_h[5] - _rtB->Product1_f[2]) -
13653 _rtB->Sum_mx[2];
13654
13655 /* Product: '<S777>/Product2' */
13656 rt_mrdivide_U1d1x3_U2d_9vOrDY_k(_rtB->Product1_f, AFCS_MODEL1_rtC(S)
13657 ->VectorConcatenate, _rtB->Sum_mx);
13658 for (i_1 = 0; i_1 < 3; i_1++) {
13659 /* Reshape: '<S777>/Reshape' */
13660 _rtB->Reshape[i_1] = _rtB->Sum_mx[i_1];
13661
13662 /* Math: '<S779>/Math Function' */
13663 _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_ak[i_1];
13664 _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_ak[i_1 + 3];
13665 _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_ak[i_1 + 6];
13666 }
13667
13668 /* Product: '<S779>/Product1' */
13669 for (i_1 = 0; i_1 < 3; i_1++) {
13670 for (flag = 0; flag < 3; flag++) {
13671 _rtB->MathFunction_k[i_1 + 3 * flag] = 0.0;
13672 _rtB->MathFunction_k[i_1 + 3 * flag] += _rtB->MathFunction1_a[3 * flag]
13673 * _rtB->VectorConcatenate_n[i_1];
13674 _rtB->MathFunction_k[i_1 + 3 * flag] += _rtB->MathFunction1_a[3 * flag
13675 + 1] * _rtB->VectorConcatenate_n[i_1 + 3];
13676 _rtB->MathFunction_k[i_1 + 3 * flag] += _rtB->MathFunction1_a[3 * flag
13677 + 2] * _rtB->VectorConcatenate_n[i_1 + 6];
13678 }
13679 }
13680
13681 /* End of Product: '<S779>/Product1' */
13682
13683 /* Math: '<S778>/Math Function' */
13684 for (i_1 = 0; i_1 < 3; i_1++) {
13685 j_2[3 * i_1] = _rtB->MathFunction_k[i_1];
13686 j_2[1 + 3 * i_1] = _rtB->MathFunction_k[i_1 + 3];
13687 j_2[2 + 3 * i_1] = _rtB->MathFunction_k[i_1 + 6];
13688 }
13689
13690 for (i_1 = 0; i_1 < 3; i_1++) {
13691 /* Product: '<S778>/Product1' */
13692 _rtB->Product1_e[i_1] = 0.0;
13693 _rtB->MathFunction_k[3 * i_1] = j_2[3 * i_1];
13694
13695 /* Math: '<S778>/Math Function1' */
13696 _rtB->MathFunction1_a[3 * i_1] = _rtB->VectorConcatenate_ak[i_1];
13697
13698 /* Math: '<S778>/Math Function2' */
13699 _rtB->MathFunction2[3 * i_1] = _rtB->VectorConcatenate_n[i_1];
13700
13701 /* Product: '<S778>/Product1' */
13702 _rtB->Product1_e[i_1] += _rtB->VectorConcatenate_ak[i_1] * _rtB->Sum_k[0];
13703 _rtB->MathFunction_k[1 + 3 * i_1] = j_2[3 * i_1 + 1];
13704
13705 /* Math: '<S778>/Math Function1' */
13706 _rtB->MathFunction1_a[1 + 3 * i_1] = _rtB->VectorConcatenate_ak[i_1 + 3];
13707
13708 /* Math: '<S778>/Math Function2' */
13709 _rtB->MathFunction2[1 + 3 * i_1] = _rtB->VectorConcatenate_n[i_1 + 3];
13710
13711 /* Product: '<S778>/Product1' */
13712 _rtB->Product1_e[i_1] += _rtB->VectorConcatenate_ak[i_1 + 3] *
13713 _rtB->Sum_k[1];
13714 _rtB->MathFunction_k[2 + 3 * i_1] = j_2[3 * i_1 + 2];
13715
13716 /* Math: '<S778>/Math Function1' */
13717 _rtB->MathFunction1_a[2 + 3 * i_1] = _rtB->VectorConcatenate_ak[i_1 + 6];
13718
13719 /* Math: '<S778>/Math Function2' */
13720 _rtB->MathFunction2[2 + 3 * i_1] = _rtB->VectorConcatenate_n[i_1 + 6];
13721
13722 /* Product: '<S778>/Product1' */
13723 _rtB->Product1_e[i_1] += _rtB->VectorConcatenate_ak[i_1 + 6] *
13724 _rtB->Sum_k[2];
13725 }
13726
13727 /* End of Math: '<S778>/Math Function' */
13728 for (i_1 = 0; i_1 < 3; i_1++) {
13729 /* Product: '<S778>/Product2' */
13730 _rtB->Sum_mx[i_1] = 0.0;
13731
13732 /* Product: '<S778>/Product4' */
13733 _rtB->Product1_f[i_1] = 0.0;
13734
13735 /* Product: '<S778>/Product5' */
13736 _rtB->Product5[i_1] = 0.0;
13737
13738 /* Product: '<S778>/Product2' */
13739 _rtB->Sum_mx[i_1] += _rtB->MathFunction_k[i_1] * _rtB->ubvbwb[0];
13740
13741 /* Product: '<S778>/Product4' */
13742 _rtB->Product1_f[i_1] += _rtB->MathFunction1_a[i_1] * 0.0;
13743
13744 /* Product: '<S778>/Product5' */
13745 _rtB->Product5[i_1] += _rtB->MathFunction2[i_1] * _rtB->ubvbwb[0];
13746
13747 /* Product: '<S778>/Product2' */
13748 _rtB->Sum_mx[i_1] += _rtB->MathFunction_k[i_1 + 3] * _rtB->ubvbwb[1];
13749
13750 /* Product: '<S778>/Product4' */
13751 _rtB->Product1_f[i_1] += _rtB->MathFunction1_a[i_1 + 3] * 0.0;
13752
13753 /* Product: '<S778>/Product5' */
13754 _rtB->Product5[i_1] += _rtB->MathFunction2[i_1 + 3] * _rtB->ubvbwb[1];
13755
13756 /* Product: '<S778>/Product2' */
13757 _rtB->Sum_mx[i_1] += _rtB->MathFunction_k[i_1 + 6] * _rtB->ubvbwb[2];
13758
13759 /* Product: '<S778>/Product4' */
13760 _rtB->Product1_f[i_1] += _rtB->MathFunction1_a[i_1 + 6] * 7.292115E-5;
13761
13762 /* Product: '<S778>/Product5' */
13763 _rtB->Product5[i_1] += _rtB->MathFunction2[i_1 + 6] * _rtB->ubvbwb[2];
13764 }
13765
13766 /* Integrator: '<S778>/p' */
13767 if (((int_T *)ssGetDWork(S, 350))[0] != 0) {
13768 _rtX->p_CSTATE[0] = _rtB->Product1_e[0];
13769 _rtX->p_CSTATE[1] = _rtB->Product1_e[1];
13770 _rtX->p_CSTATE[2] = _rtB->Product1_e[2];
13771 }
13772
13773 _rtB->p_p[0] = _rtX->p_CSTATE[0];
13774 _rtB->p_p[1] = _rtX->p_CSTATE[1];
13775 _rtB->p_p[2] = _rtX->p_CSTATE[2];
13776
13777 /* End of Integrator: '<S778>/p' */
13778
13779 /* Product: '<S842>/j x k' */
13780 _rtB->jxk = _rtB->p_p[1] * _rtB->Product1_f[2];
13781
13782 /* Product: '<S842>/k x i' */
13783 _rtB->kxi = _rtB->p_p[2] * _rtB->Product1_f[0];
13784
13785 /* Product: '<S842>/i x j' */
13786 _rtB->ixj = _rtB->p_p[0] * _rtB->Product1_f[1];
13787
13788 /* Product: '<S843>/k x j' */
13789 _rtB->kxj = _rtB->p_p[2] * _rtB->Product1_f[1];
13790
13791 /* Product: '<S843>/i x k' */
13792 _rtB->ixk_a = _rtB->p_p[0] * _rtB->Product1_f[2];
13793
13794 /* Product: '<S843>/j x i' */
13795 _rtB->jxi_p = _rtB->p_p[1] * _rtB->Product1_f[0];
13796
13797 /* Sum: '<S801>/Sum' */
13798 _rtB->Product1_f[0] = _rtB->jxk - _rtB->kxj;
13799 _rtB->Product1_f[1] = _rtB->kxi - _rtB->ixk_a;
13800 _rtB->Product1_f[2] = _rtB->ixj - _rtB->jxi_p;
13801
13802 /* Sum: '<S778>/Sum2' */
13803 _rtB->Sum2_e[0] = _rtB->Sum_mx[0] - _rtB->Product1_f[0];
13804 _rtB->Sum2_e[1] = _rtB->Sum_mx[1] - _rtB->Product1_f[1];
13805 _rtB->Sum2_e[2] = _rtB->Sum_mx[2] - _rtB->Product1_f[2];
13806
13807 /* Sum: '<S904>/Add' */
13808 _rtB->Add_n = (_rtB->Product4[0] + _rtB->Product4[4]) + _rtB->Product4[8];
13809
13810 /* If: '<S848>/If' */
13811 if (ssIsMajorTimeStep(S)) {
13812 rtAction = (int8_T)!(_rtB->Add_n > 0.0);
13813 ((int8_T *)ssGetDWork(S, 508))[0] = rtAction;
13814 } else {
13815 rtAction = ((int8_T *)ssGetDWork(S, 508))[0];
13816 }
13817
13818 switch (rtAction) {
13819 case 0:
13820 /* Outputs for IfAction SubSystem: '<S848>/Positive Trace' incorporates:
13821 * ActionPort: '<S903>/Action Port'
13822 */
13823 /* Sum: '<S903>/Sum' incorporates:
13824 * Constant: '<S903>/Constant'
13825 */
13826 _rtB->Gain1_g = _rtB->Add_n + 1.0;
13827
13828 /* Sqrt: '<S903>/sqrt' */
13829 _rtB->Gain1_g = sqrt(_rtB->Gain1_g);
13830
13831 /* Gain: '<S903>/Gain1' */
13832 _rtB->Gain1_g *= 2.0;
13833
13834 /* Sum: '<S925>/Add' */
13835 _rtB->Add_a = _rtB->Product4[7] - _rtB->Product4[5];
13836
13837 /* Sum: '<S924>/Add' */
13838 _rtB->Add_nms = _rtB->Product4[2] - _rtB->Product4[6];
13839
13840 /* Sum: '<S926>/Add' */
13841 _rtB->Add_fy = _rtB->Product4[3] - _rtB->Product4[1];
13842
13843 /* End of Outputs for SubSystem: '<S848>/Positive Trace' */
13844 break;
13845
13846 case 1:
13847 /* Outputs for IfAction SubSystem: '<S848>/Negative Trace' incorporates:
13848 * ActionPort: '<S902>/Action Port'
13849 */
13850 /* If: '<S902>/Find Maximum Diagonal Value' */
13851 if (ssIsMajorTimeStep(S)) {
13852 if ((_rtB->Product4[4] > _rtB->Product4[0]) && (_rtB->Product4[4] >
13853 _rtB->Product4[8])) {
13854 rtAction = 0;
13855 } else if (_rtB->Product4[8] > _rtB->Product4[0]) {
13856 rtAction = 1;
13857 } else {
13858 rtAction = 2;
13859 }
13860
13861 ((int8_T *)ssGetDWork(S, 509))[0] = rtAction;
13862 } else {
13863 rtAction = ((int8_T *)ssGetDWork(S, 509))[0];
13864 }
13865
13866 switch (rtAction) {
13867 case 0:
13868 /* Outputs for IfAction SubSystem: '<S902>/Maximum Value at DCM(2,2)' incorporates:
13869 * ActionPort: '<S906>/Action Port'
13870 */
13871 /* Sum: '<S918>/Add' incorporates:
13872 * Constant: '<S918>/Constant'
13873 */
13874 _rtB->Product_pv = ((_rtB->Product4[4] - _rtB->Product4[0]) -
13875 _rtB->Product4[8]) + 1.0;
13876
13877 /* Sqrt: '<S906>/sqrt' */
13878 _rtB->Product_pv = sqrt(_rtB->Product_pv);
13879
13880 /* Sum: '<S916>/Add' */
13881 _rtB->Add_j = _rtB->Product4[1] + _rtB->Product4[3];
13882
13883 /* Sum: '<S915>/Add' */
13884 _rtB->Add_e = _rtB->Product4[5] + _rtB->Product4[7];
13885
13886 /* Sum: '<S914>/Add' */
13887 _rtB->Add_g = _rtB->Product4[2] - _rtB->Product4[6];
13888
13889 /* Switch: '<S917>/Switch' incorporates:
13890 * Constant: '<S917>/Constant1'
13891 */
13892 if (_rtB->Product_pv != 0.0) {
13893 _rtB->Switch_j[0] = 0.5;
13894 _rtB->Switch_j[1] = _rtB->Product_pv;
13895 } else {
13896 _rtB->Switch_j[0] = 0.0;
13897 _rtB->Switch_j[1] = 1.0;
13898 }
13899
13900 /* End of Switch: '<S917>/Switch' */
13901
13902 /* Product: '<S917>/Product' */
13903 _rtB->Product_pv = _rtB->Switch_j[0] / _rtB->Switch_j[1];
13904
13905 /* Product: '<S906>/Product' */
13906 _rtB->Product_i[0] = _rtB->Add_j * _rtB->Product_pv;
13907 _rtB->Product_i[1] = _rtB->Add_e * _rtB->Product_pv;
13908 _rtB->Product_i[2] = _rtB->Add_g * _rtB->Product_pv;
13909
13910 /* End of Outputs for SubSystem: '<S902>/Maximum Value at DCM(2,2)' */
13911 break;
13912
13913 case 1:
13914 /* Outputs for IfAction SubSystem: '<S902>/Maximum Value at DCM(3,3)' incorporates:
13915 * ActionPort: '<S907>/Action Port'
13916 */
13917 /* Sum: '<S923>/Add' incorporates:
13918 * Constant: '<S923>/Constant'
13919 */
13920 _rtB->Product_l = ((_rtB->Product4[8] - _rtB->Product4[0]) -
13921 _rtB->Product4[4]) + 1.0;
13922
13923 /* Sqrt: '<S907>/sqrt' */
13924 _rtB->Product_l = sqrt(_rtB->Product_l);
13925
13926 /* Sum: '<S919>/Add' */
13927 _rtB->Add_nm = _rtB->Product4[2] + _rtB->Product4[6];
13928
13929 /* Sum: '<S920>/Add' */
13930 _rtB->Add_f = _rtB->Product4[5] + _rtB->Product4[7];
13931
13932 /* Sum: '<S921>/Add' */
13933 _rtB->Add_c = _rtB->Product4[3] - _rtB->Product4[1];
13934
13935 /* Switch: '<S922>/Switch' incorporates:
13936 * Constant: '<S922>/Constant1'
13937 */
13938 if (_rtB->Product_l != 0.0) {
13939 _rtB->Switch_d[0] = 0.5;
13940 _rtB->Switch_d[1] = _rtB->Product_l;
13941 } else {
13942 _rtB->Switch_d[0] = 0.0;
13943 _rtB->Switch_d[1] = 1.0;
13944 }
13945
13946 /* End of Switch: '<S922>/Switch' */
13947
13948 /* Product: '<S922>/Product' */
13949 _rtB->Product_l = _rtB->Switch_d[0] / _rtB->Switch_d[1];
13950
13951 /* Product: '<S907>/Product' */
13952 _rtB->Product_o[0] = _rtB->Add_nm * _rtB->Product_l;
13953 _rtB->Product_o[1] = _rtB->Add_f * _rtB->Product_l;
13954 _rtB->Product_o[2] = _rtB->Add_c * _rtB->Product_l;
13955
13956 /* End of Outputs for SubSystem: '<S902>/Maximum Value at DCM(3,3)' */
13957 break;
13958
13959 case 2:
13960 /* Outputs for IfAction SubSystem: '<S902>/Maximum Value at DCM(1,1)' incorporates:
13961 * ActionPort: '<S905>/Action Port'
13962 */
13963 /* Sum: '<S913>/Add' incorporates:
13964 * Constant: '<S913>/Constant'
13965 */
13966 _rtB->Product_m = ((_rtB->Product4[0] - _rtB->Product4[4]) -
13967 _rtB->Product4[8]) + 1.0;
13968
13969 /* Sqrt: '<S905>/sqrt' */
13970 _rtB->Product_m = sqrt(_rtB->Product_m);
13971
13972 /* Switch: '<S912>/Switch' incorporates:
13973 * Constant: '<S912>/Constant1'
13974 */
13975 if (_rtB->Product_m != 0.0) {
13976 _rtB->Switch_f[0] = 0.5;
13977 _rtB->Switch_f[1] = _rtB->Product_m;
13978 } else {
13979 _rtB->Switch_f[0] = 0.0;
13980 _rtB->Switch_f[1] = 1.0;
13981 }
13982
13983 /* End of Switch: '<S912>/Switch' */
13984
13985 /* Product: '<S912>/Product' */
13986 _rtB->Product_m = _rtB->Switch_f[0] / _rtB->Switch_f[1];
13987
13988 /* Sum: '<S911>/Add' */
13989 _rtB->Add_k = _rtB->Product4[1] + _rtB->Product4[3];
13990
13991 /* Sum: '<S909>/Add' */
13992 _rtB->Add_kg = _rtB->Product4[2] + _rtB->Product4[6];
13993
13994 /* Sum: '<S910>/Add' */
13995 _rtB->Add_p = _rtB->Product4[7] - _rtB->Product4[5];
13996
13997 /* Product: '<S905>/Product' */
13998 _rtB->Product_gm[0] = _rtB->Product_m * _rtB->Add_k;
13999 _rtB->Product_gm[1] = _rtB->Product_m * _rtB->Add_kg;
14000 _rtB->Product_gm[2] = _rtB->Product_m * _rtB->Add_p;
14001
14002 /* End of Outputs for SubSystem: '<S902>/Maximum Value at DCM(1,1)' */
14003 break;
14004 }
14005
14006 /* End of If: '<S902>/Find Maximum Diagonal Value' */
14007 /* End of Outputs for SubSystem: '<S848>/Negative Trace' */
14008 break;
14009 }
14010
14011 /* End of If: '<S848>/If' */
14012
14013 /* DotProduct: '<S851>/Dot Product' */
14014 _rtB->jxk = ((_rtB->q_j[0] * _rtB->q_j[0] + _rtB->q_j[1] * _rtB->q_j[1]) +
14015 _rtB->q_j[2] * _rtB->q_j[2]) + _rtB->q_j[3] * _rtB->q_j[3];
14016
14017 /* Sum: '<S851>/Sum' incorporates:
14018 * Constant: '<S851>/Constant'
14019 */
14020 _rtB->jxk = 1.0 - _rtB->jxk;
14021
14022 /* Sum: '<S470>/Sum4' */
14023 _rtB->Sum4[0] = _rtB->Sum2_h[0] - _rtB->Sum4[0];
14024 _rtB->Sum4[1] = _rtB->Sum2_h[1] - _rtB->Sum4[1];
14025 _rtB->Sum4[2] = _rtB->Sum2_h[2] - _rtB->Sum4[2];
14026
14027 /* Fcn: '<S851>/q0dot' */
14028 _rtB->q0dot = ((_rtB->q_j[1] * _rtB->Sum4[0] + _rtB->q_j[2] * _rtB->Sum4[1])
14029 + _rtB->q_j[3] * _rtB->Sum4[2]) * -0.5 + _rtB->jxk *
14030 _rtB->q_j[0];
14031
14032 /* Fcn: '<S851>/q1dot' */
14033 _rtB->q1dot = ((_rtB->q_j[0] * _rtB->Sum4[0] + _rtB->q_j[2] * _rtB->Sum4[2])
14034 - _rtB->q_j[3] * _rtB->Sum4[1]) * 0.5 + _rtB->jxk *
14035 _rtB->q_j[1];
14036
14037 /* Fcn: '<S851>/q2dot' */
14038 _rtB->q2dot = ((_rtB->q_j[0] * _rtB->Sum4[1] + _rtB->q_j[3] * _rtB->Sum4[0])
14039 - _rtB->q_j[1] * _rtB->Sum4[2]) * 0.5 + _rtB->jxk *
14040 _rtB->q_j[2];
14041
14042 /* Fcn: '<S851>/q3dot' */
14043 _rtB->q3dot = ((_rtB->q_j[0] * _rtB->Sum4[2] + _rtB->q_j[1] * _rtB->Sum4[1])
14044 - _rtB->q_j[2] * _rtB->Sum4[0]) * 0.5 + _rtB->jxk *
14045 _rtB->q_j[3];
14046
14047 /* Product: '<S948>/j x k' */
14048 _rtB->jxk = 0.0 * _rtB->p1[2];
14049
14050 /* Product: '<S948>/k x i' */
14051 _rtB->kxi = 7.292115E-5 * _rtB->p1[0];
14052
14053 /* Product: '<S948>/i x j' */
14054 _rtB->ixj = 0.0 * _rtB->p1[1];
14055
14056 /* Product: '<S949>/k x j' */
14057 _rtB->kxj = 7.292115E-5 * _rtB->p1[1];
14058
14059 /* Product: '<S949>/i x k' */
14060 _rtB->ixk_a = 0.0 * _rtB->p1[2];
14061
14062 /* Product: '<S949>/j x i' */
14063 _rtB->jxi_p = 0.0 * _rtB->p1[0];
14064
14065 /* Sum: '<S943>/Sum' */
14066 _rtB->Sum_mx[0] = _rtB->jxk - _rtB->kxj;
14067 _rtB->Sum_mx[1] = _rtB->kxi - _rtB->ixk_a;
14068 _rtB->Sum_mx[2] = _rtB->ixj - _rtB->jxi_p;
14069
14070 /* Product: '<S946>/j x k' */
14071 _rtB->jxk = 0.0 * _rtB->Sum_mx[2];
14072
14073 /* Product: '<S946>/k x i' */
14074 _rtB->kxi = 7.292115E-5 * _rtB->Sum_mx[0];
14075
14076 /* Product: '<S946>/i x j' */
14077 _rtB->ixj = 0.0 * _rtB->Sum_mx[1];
14078
14079 /* Product: '<S947>/k x j' */
14080 _rtB->kxj = 7.292115E-5 * _rtB->Sum_mx[1];
14081
14082 /* Product: '<S947>/i x k' */
14083 _rtB->ixk_a = 0.0 * _rtB->Sum_mx[2];
14084
14085 /* Product: '<S947>/j x i' */
14086 _rtB->jxi_p = 0.0 * _rtB->Sum_mx[0];
14087
14088 /* Sum: '<S942>/Sum' */
14089 _rtB->Sum_mx[0] = _rtB->jxk - _rtB->kxj;
14090 _rtB->Sum_mx[1] = _rtB->kxi - _rtB->ixk_a;
14091 _rtB->Sum_mx[2] = _rtB->ixj - _rtB->jxi_p;
14092
14093 /* Product: '<S780>/Product1' */
14094 for (i_1 = 0; i_1 < 3; i_1++) {
14095 _rtB->Product1_f[i_1] = 0.0;
14096 _rtB->Product1_f[i_1] += _rtB->VectorConcatenate_n[i_1] * _rtB->Sum_mx[0];
14097 _rtB->Product1_f[i_1] += _rtB->VectorConcatenate_n[i_1 + 3] *
14098 _rtB->Sum_mx[1];
14099 _rtB->Product1_f[i_1] += _rtB->VectorConcatenate_n[i_1 + 6] *
14100 _rtB->Sum_mx[2];
14101 }
14102
14103 /* End of Product: '<S780>/Product1' */
14104 for (i_1 = 0; i_1 < 3; i_1++) {
14105 /* Product: '<S780>/Product2' */
14106 _rtB->Sum_mx[i_1] = 0.0;
14107 _rtB->Sum_mx[i_1] += _rtB->VectorConcatenate_n[i_1] * 0.0;
14108 _rtB->Sum_mx[i_1] += _rtB->VectorConcatenate_n[i_1 + 3] * 0.0;
14109 _rtB->Sum_mx[i_1] += _rtB->VectorConcatenate_n[i_1 + 6] * 7.292115E-5;
14110
14111 /* Sum: '<S780>/Sum2' */
14112 _rtB->Sum2_h[i_1] += _rtB->Sum_mx[i_1];
14113 }
14114
14115 /* Product: '<S944>/j x k' */
14116 _rtB->jxk = _rtB->ubvbwb[1] * _rtB->Sum2_h[2];
14117
14118 /* Product: '<S944>/k x i' */
14119 _rtB->kxi = _rtB->ubvbwb[2] * _rtB->Sum2_h[0];
14120
14121 /* Product: '<S944>/i x j' */
14122 _rtB->ixj = _rtB->ubvbwb[0] * _rtB->Sum2_h[1];
14123
14124 /* Product: '<S945>/k x j' */
14125 _rtB->kxj = _rtB->ubvbwb[2] * _rtB->Sum2_h[1];
14126
14127 /* Product: '<S945>/i x k' */
14128 _rtB->ixk_a = _rtB->ubvbwb[0] * _rtB->Sum2_h[2];
14129
14130 /* Product: '<S945>/j x i' */
14131 _rtB->jxi_p = _rtB->ubvbwb[1] * _rtB->Sum2_h[0];
14132
14133 /* Sum: '<S941>/Sum' */
14134 _rtB->Sum_mx[0] = _rtB->jxk - _rtB->kxj;
14135 _rtB->Sum_mx[1] = _rtB->kxi - _rtB->ixk_a;
14136 _rtB->Sum_mx[2] = _rtB->ixj - _rtB->jxi_p;
14137
14138 /* Sum: '<S780>/Sum' */
14139 _rtB->Sum_e[0] = (_rtB->Sum_mx[0] - _rtB->Product1_f[0]) + _rtB->
14140 Product_d[0];
14141 _rtB->Sum_e[1] = (_rtB->Sum_mx[1] - _rtB->Product1_f[1]) + _rtB->
14142 Product_d[1];
14143 _rtB->Sum_e[2] = (_rtB->Sum_mx[2] - _rtB->Product1_f[2]) + _rtB->
14144 Product_d[2];
14145
14146 /* TransferFcn: '<S446>/Transfer Fcn' */
14147 _rtB->TransferFcn = 0.0;
14148 _rtB->TransferFcn += -2500.0 * _rtX->TransferFcn_CSTATE;
14149 _rtB->TransferFcn += 50.0 * _rtB->Switch_h;
14150 }
14151 }
14152
14153 /* End of Outputs for SubSystem: '<S15>/Subsystem3' */
14154 if (ssIsSampleHit(S, 1, 0)) {
14155 }
14156
14157 if (1) {
14158 /* Gain: '<S448>/Gain14' */
14159 _rtB->Gain14 = 1000.0 * _rtB->Altitude_j;
14160 }
14161
14162 if (ssIsSampleHit(S, 1, 0)) {
14163 /* Gain: '<S449>/Gain14' */
14164 _rtB->Gain14_b = 1000.0 * _rtB->Gain11;
14165
14166 /* Gain: '<S449>/Gain12' incorporates:
14167 * Constant: '<S14>/Constant2'
14168 */
14169 _rtB->Gain13_d = 4.0E+8;
14170
14171 /* DataTypeConversion: '<S449>/Data Type Conversion13' */
14172 TrigonometricFunction4_o = floor(_rtB->Gain13_d);
14173 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14174 {
14175 TrigonometricFunction4_o = 0.0;
14176 } else {
14177 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14178 }
14179
14180 _rtB->Lat_c = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14181 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14182
14183 /* End of DataTypeConversion: '<S449>/Data Type Conversion13' */
14184
14185 /* Gain: '<S449>/Gain13' incorporates:
14186 * Constant: '<S14>/Constant1'
14187 */
14188 _rtB->Gain13_d = 3.0E+8;
14189
14190 /* DataTypeConversion: '<S449>/Data Type Conversion14' */
14191 TrigonometricFunction4_o = floor(_rtB->Gain13_d);
14192 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14193 {
14194 TrigonometricFunction4_o = 0.0;
14195 } else {
14196 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14197 }
14198
14199 _rtB->Lon_a = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14200 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14201
14202 /* End of DataTypeConversion: '<S449>/Data Type Conversion14' */
14203
14204 /* DataTypeConversion: '<S449>/Data Type Conversion15' */
14205 TrigonometricFunction4_o = floor(_rtB->Gain14_b);
14206 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14207 {
14208 TrigonometricFunction4_o = 0.0;
14209 } else {
14210 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14211 }
14212
14213 _rtB->Alt_e = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14214 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14215
14216 /* End of DataTypeConversion: '<S449>/Data Type Conversion15' */
14217 }
14218
14219 if (1) {
14220 /* Switch: '<S445>/Switch' incorporates:
14221 * Switch: '<S445>/Switch'
14222 */
14223 if (_rtB->takeoff > 0) {
14224 /* Gain: '<S448>/Gain' */
14225 _rtB->Gain_d = 1000.0 * _rtB->phiL;
14226
14227 /* DataTypeConversion: '<S448>/Data Type Conversion10' */
14228 TrigonometricFunction4_o = floor(_rtB->Gain_d);
14229 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14230 {
14231 TrigonometricFunction4_o = 0.0;
14232 } else {
14233 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14234 }
14235
14236 _rtB->phiL_k = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14237 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14238
14239 /* End of DataTypeConversion: '<S448>/Data Type Conversion10' */
14240 _rtB->phiL_j = _rtB->phiL_k;
14241
14242 /* Gain: '<S448>/Gain9' */
14243 _rtB->Gain9 = 1000.0 * _rtB->GPS_vnorth_i;
14244
14245 /* DataTypeConversion: '<S448>/Data Type Conversion9' */
14246 TrigonometricFunction4_o = floor(_rtB->Gain9);
14247 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14248 {
14249 TrigonometricFunction4_o = 0.0;
14250 } else {
14251 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14252 }
14253
14254 _rtB->Vn_c = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14255 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14256
14257 /* End of DataTypeConversion: '<S448>/Data Type Conversion9' */
14258 _rtB->Vn = _rtB->Vn_c;
14259
14260 /* Gain: '<S448>/Gain10' */
14261 _rtB->Gain10 = 1000.0 * _rtB->GPS_veast_k;
14262
14263 /* DataTypeConversion: '<S448>/Data Type Conversion11' */
14264 TrigonometricFunction4_o = floor(_rtB->Gain10);
14265 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14266 {
14267 TrigonometricFunction4_o = 0.0;
14268 } else {
14269 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14270 }
14271
14272 _rtB->Ve_a = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14273 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14274
14275 /* End of DataTypeConversion: '<S448>/Data Type Conversion11' */
14276 _rtB->Ve = _rtB->Ve_a;
14277
14278 /* Gain: '<S448>/Gain11' */
14279 _rtB->Gain11_h = 1000.0 * _rtB->GPS_vdown_k;
14280
14281 /* DataTypeConversion: '<S448>/Data Type Conversion12' */
14282 TrigonometricFunction4_o = floor(_rtB->Gain11_h);
14283 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14284 {
14285 TrigonometricFunction4_o = 0.0;
14286 } else {
14287 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14288 }
14289
14290 _rtB->Vd_j = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14291 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14292
14293 /* End of DataTypeConversion: '<S448>/Data Type Conversion12' */
14294 _rtB->Vd = _rtB->Vd_j;
14295
14296 /* Gain: '<S448>/Gain12' */
14297 _rtB->Gain12 = 1.0E+7 * _rtB->Lat_j;
14298
14299 /* DataTypeConversion: '<S448>/Data Type Conversion13' */
14300 TrigonometricFunction4_o = floor(_rtB->Gain12);
14301 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14302 {
14303 TrigonometricFunction4_o = 0.0;
14304 } else {
14305 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14306 }
14307
14308 _rtB->Lat_i = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14309 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14310
14311 /* End of DataTypeConversion: '<S448>/Data Type Conversion13' */
14312 _rtB->Lat_pm = _rtB->Lat_i;
14313
14314 /* Gain: '<S448>/Gain13' */
14315 _rtB->Gain13 = 1.0E+7 * _rtB->Lon_e;
14316
14317 /* DataTypeConversion: '<S448>/Data Type Conversion14' */
14318 TrigonometricFunction4_o = floor(_rtB->Gain13);
14319 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14320 {
14321 TrigonometricFunction4_o = 0.0;
14322 } else {
14323 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14324 }
14325
14326 _rtB->Lon_eh = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14327 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14328
14329 /* End of DataTypeConversion: '<S448>/Data Type Conversion14' */
14330 _rtB->Lon_m = _rtB->Lon_eh;
14331
14332 /* DataTypeConversion: '<S448>/Data Type Conversion15' */
14333 TrigonometricFunction4_o = floor(_rtB->Gain14);
14334 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14335 {
14336 TrigonometricFunction4_o = 0.0;
14337 } else {
14338 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14339 }
14340
14341 _rtB->Alt_oj = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14342 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14343
14344 /* End of DataTypeConversion: '<S448>/Data Type Conversion15' */
14345 _rtB->Alt_o3 = _rtB->Alt_oj;
14346
14347 /* Gain: '<S448>/Gain15' */
14348 _rtB->Gain15 = 1000.0 * _rtB->Alfa;
14349
14350 /* DataTypeConversion: '<S448>/Data Type Conversion16' */
14351 TrigonometricFunction4_o = floor(_rtB->Gain15);
14352 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14353 {
14354 TrigonometricFunction4_o = 0.0;
14355 } else {
14356 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14357 }
14358
14359 _rtB->Ndefine1_k = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14360 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14361
14362 /* End of DataTypeConversion: '<S448>/Data Type Conversion16' */
14363 _rtB->Ndefine1 = _rtB->Ndefine1_k;
14364
14365 /* Gain: '<S448>/Gain16' */
14366 _rtB->Gain16 = 1000.0 * _rtB->Beta;
14367
14368 /* DataTypeConversion: '<S448>/Data Type Conversion17' */
14369 TrigonometricFunction4_o = floor(_rtB->Gain16);
14370 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14371 {
14372 TrigonometricFunction4_o = 0.0;
14373 } else {
14374 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14375 }
14376
14377 _rtB->Ndefine2_o = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14378 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14379
14380 /* End of DataTypeConversion: '<S448>/Data Type Conversion17' */
14381 _rtB->Ndefine2 = _rtB->Ndefine2_o;
14382
14383 /* Gain: '<S448>/Gain17' */
14384 _rtB->Gain17 = 1000.0 * _rtB->Qbar;
14385
14386 /* DataTypeConversion: '<S448>/Data Type Conversion18' */
14387 TrigonometricFunction4_o = floor(_rtB->Gain17);
14388 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14389 {
14390 TrigonometricFunction4_o = 0.0;
14391 } else {
14392 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14393 }
14394
14395 _rtB->Ndefine3_c = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14396 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14397
14398 /* End of DataTypeConversion: '<S448>/Data Type Conversion18' */
14399 _rtB->Ndefine3 = _rtB->Ndefine3_c;
14400
14401 /* Gain: '<S448>/Gain18' */
14402 _rtB->Gain18 = 1000.0 * _rtB->Va;
14403
14404 /* DataTypeConversion: '<S448>/Data Type Conversion19' */
14405 TrigonometricFunction4_o = floor(_rtB->Gain18);
14406 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14407 {
14408 TrigonometricFunction4_o = 0.0;
14409 } else {
14410 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14411 }
14412
14413 _rtB->Va_m = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14414 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14415
14416 /* End of DataTypeConversion: '<S448>/Data Type Conversion19' */
14417 _rtB->Va_k = _rtB->Va_m;
14418
14419 /* Gain: '<S448>/Gain1' */
14420 _rtB->Gain1_p = 1000.0 * _rtB->thetaL;
14421
14422 /* DataTypeConversion: '<S448>/Data Type Conversion1' */
14423 TrigonometricFunction4_o = floor(_rtB->Gain1_p);
14424 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14425 {
14426 TrigonometricFunction4_o = 0.0;
14427 } else {
14428 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14429 }
14430
14431 _rtB->thetaL_m = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14432 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14433
14434 /* End of DataTypeConversion: '<S448>/Data Type Conversion1' */
14435 _rtB->thetaL_d = _rtB->thetaL_m;
14436
14437 /* Gain: '<S448>/Gain2' */
14438 _rtB->Gain2_i = 1000.0 * _rtB->psiL;
14439
14440 /* DataTypeConversion: '<S448>/Data Type Conversion2' */
14441 TrigonometricFunction4_o = floor(_rtB->Gain2_i);
14442 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14443 {
14444 TrigonometricFunction4_o = 0.0;
14445 } else {
14446 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14447 }
14448
14449 _rtB->psiL_f = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14450 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14451
14452 /* End of DataTypeConversion: '<S448>/Data Type Conversion2' */
14453 _rtB->psiL_j = _rtB->psiL_f;
14454
14455 /* Gain: '<S448>/Gain3' */
14456 _rtB->Gain3_b = 1000.0 * _rtB->p;
14457
14458 /* DataTypeConversion: '<S448>/Data Type Conversion3' */
14459 TrigonometricFunction4_o = floor(_rtB->Gain3_b);
14460 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14461 {
14462 TrigonometricFunction4_o = 0.0;
14463 } else {
14464 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14465 }
14466
14467 _rtB->p_l = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14468 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14469
14470 /* End of DataTypeConversion: '<S448>/Data Type Conversion3' */
14471 _rtB->p_pm = _rtB->p_l;
14472
14473 /* Gain: '<S448>/Gain4' */
14474 _rtB->Gain4 = 1000.0 * _rtB->q;
14475
14476 /* DataTypeConversion: '<S448>/Data Type Conversion4' */
14477 TrigonometricFunction4_o = floor(_rtB->Gain4);
14478 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14479 {
14480 TrigonometricFunction4_o = 0.0;
14481 } else {
14482 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14483 }
14484
14485 _rtB->q_g = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14486 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14487
14488 /* End of DataTypeConversion: '<S448>/Data Type Conversion4' */
14489 _rtB->q_je = _rtB->q_g;
14490
14491 /* Gain: '<S448>/Gain5' */
14492 _rtB->Gain5 = 1000.0 * _rtB->r;
14493
14494 /* DataTypeConversion: '<S448>/Data Type Conversion5' */
14495 TrigonometricFunction4_o = floor(_rtB->Gain5);
14496 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14497 {
14498 TrigonometricFunction4_o = 0.0;
14499 } else {
14500 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14501 }
14502
14503 _rtB->r_o = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14504 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14505
14506 /* End of DataTypeConversion: '<S448>/Data Type Conversion5' */
14507 _rtB->r_l = _rtB->r_o;
14508
14509 /* Gain: '<S448>/Gain6' */
14510 _rtB->Gain6 = 1000.0 * _rtB->Abx;
14511
14512 /* DataTypeConversion: '<S448>/Data Type Conversion6' */
14513 TrigonometricFunction4_o = floor(_rtB->Gain6);
14514 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14515 {
14516 TrigonometricFunction4_o = 0.0;
14517 } else {
14518 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14519 }
14520
14521 _rtB->Abx_n = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14522 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14523
14524 /* End of DataTypeConversion: '<S448>/Data Type Conversion6' */
14525 _rtB->Abx_o = _rtB->Abx_n;
14526
14527 /* Gain: '<S448>/Gain7' */
14528 _rtB->Gain7 = 1000.0 * _rtB->Aby;
14529
14530 /* DataTypeConversion: '<S448>/Data Type Conversion7' */
14531 TrigonometricFunction4_o = floor(_rtB->Gain7);
14532 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14533 {
14534 TrigonometricFunction4_o = 0.0;
14535 } else {
14536 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14537 }
14538
14539 _rtB->Aby_l = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14540 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14541
14542 /* End of DataTypeConversion: '<S448>/Data Type Conversion7' */
14543 _rtB->Aby_a = _rtB->Aby_l;
14544
14545 /* Gain: '<S448>/Gain8' */
14546 _rtB->Gain8 = 1000.0 * _rtB->Abz;
14547
14548 /* DataTypeConversion: '<S448>/Data Type Conversion8' */
14549 TrigonometricFunction4_o = floor(_rtB->Gain8);
14550 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14551 {
14552 TrigonometricFunction4_o = 0.0;
14553 } else {
14554 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14555 }
14556
14557 _rtB->Abz_d = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14558 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14559
14560 /* End of DataTypeConversion: '<S448>/Data Type Conversion8' */
14561 _rtB->Abz_h = _rtB->Abz_d;
14562 } else {
14563 /* Gain: '<S449>/Gain' */
14564 _rtB->Gain_a = 1000.0 * _rtB->phiL;
14565
14566 /* DataTypeConversion: '<S449>/Data Type Conversion10' */
14567 TrigonometricFunction4_o = floor(_rtB->Gain_a);
14568 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14569 {
14570 TrigonometricFunction4_o = 0.0;
14571 } else {
14572 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14573 }
14574
14575 _rtB->phiL_d = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14576 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14577
14578 /* End of DataTypeConversion: '<S449>/Data Type Conversion10' */
14579 _rtB->phiL_j = _rtB->phiL_d;
14580
14581 /* Gain: '<S449>/Gain9' */
14582 _rtB->Gain9_j = 1000.0 * _rtB->GPS_vnorth_i;
14583
14584 /* DataTypeConversion: '<S449>/Data Type Conversion9' */
14585 TrigonometricFunction4_o = floor(_rtB->Gain9_j);
14586 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14587 {
14588 TrigonometricFunction4_o = 0.0;
14589 } else {
14590 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14591 }
14592
14593 _rtB->Vn_m = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14594 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14595
14596 /* End of DataTypeConversion: '<S449>/Data Type Conversion9' */
14597 _rtB->Vn = _rtB->Vn_m;
14598
14599 /* Gain: '<S449>/Gain10' */
14600 _rtB->Gain10_k = 1000.0 * _rtB->GPS_veast_k;
14601
14602 /* DataTypeConversion: '<S449>/Data Type Conversion11' */
14603 TrigonometricFunction4_o = floor(_rtB->Gain10_k);
14604 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14605 {
14606 TrigonometricFunction4_o = 0.0;
14607 } else {
14608 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14609 }
14610
14611 _rtB->Ve_p = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14612 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14613
14614 /* End of DataTypeConversion: '<S449>/Data Type Conversion11' */
14615 _rtB->Ve = _rtB->Ve_p;
14616
14617 /* Gain: '<S449>/Gain11' */
14618 _rtB->Gain11_a = 1000.0 * _rtB->GPS_vdown_k;
14619
14620 /* DataTypeConversion: '<S449>/Data Type Conversion12' */
14621 TrigonometricFunction4_o = floor(_rtB->Gain11_a);
14622 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14623 {
14624 TrigonometricFunction4_o = 0.0;
14625 } else {
14626 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14627 }
14628
14629 _rtB->Vd_o = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14630 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14631
14632 /* End of DataTypeConversion: '<S449>/Data Type Conversion12' */
14633 _rtB->Vd = _rtB->Vd_o;
14634 _rtB->Lat_pm = _rtB->Lat_c;
14635 _rtB->Lon_m = _rtB->Lon_a;
14636 _rtB->Alt_o3 = _rtB->Alt_e;
14637
14638 /* Gain: '<S449>/Gain15' */
14639 _rtB->Gain15_n = 1000.0 * _rtB->Alfa;
14640
14641 /* DataTypeConversion: '<S449>/Data Type Conversion16' */
14642 TrigonometricFunction4_o = floor(_rtB->Gain15_n);
14643 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14644 {
14645 TrigonometricFunction4_o = 0.0;
14646 } else {
14647 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14648 }
14649
14650 _rtB->Ndefine1_o = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14651 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14652
14653 /* End of DataTypeConversion: '<S449>/Data Type Conversion16' */
14654 _rtB->Ndefine1 = _rtB->Ndefine1_o;
14655
14656 /* Gain: '<S449>/Gain16' */
14657 _rtB->Gain16_d = 1000.0 * _rtB->Beta;
14658
14659 /* DataTypeConversion: '<S449>/Data Type Conversion17' */
14660 TrigonometricFunction4_o = floor(_rtB->Gain16_d);
14661 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14662 {
14663 TrigonometricFunction4_o = 0.0;
14664 } else {
14665 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14666 }
14667
14668 _rtB->Ndefine2_m = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14669 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14670
14671 /* End of DataTypeConversion: '<S449>/Data Type Conversion17' */
14672 _rtB->Ndefine2 = _rtB->Ndefine2_m;
14673
14674 /* Gain: '<S449>/Gain17' */
14675 _rtB->Gain17_i = 1000.0 * _rtB->Qbar;
14676
14677 /* DataTypeConversion: '<S449>/Data Type Conversion18' */
14678 TrigonometricFunction4_o = floor(_rtB->Gain17_i);
14679 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14680 {
14681 TrigonometricFunction4_o = 0.0;
14682 } else {
14683 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14684 }
14685
14686 _rtB->Ndefine3_a = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14687 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14688
14689 /* End of DataTypeConversion: '<S449>/Data Type Conversion18' */
14690 _rtB->Ndefine3 = _rtB->Ndefine3_a;
14691
14692 /* Gain: '<S449>/Gain18' */
14693 _rtB->Gain18_b = 1000.0 * _rtB->Va;
14694
14695 /* DataTypeConversion: '<S449>/Data Type Conversion19' */
14696 TrigonometricFunction4_o = floor(_rtB->Gain18_b);
14697 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14698 {
14699 TrigonometricFunction4_o = 0.0;
14700 } else {
14701 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14702 }
14703
14704 _rtB->Va_f = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14705 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14706
14707 /* End of DataTypeConversion: '<S449>/Data Type Conversion19' */
14708 _rtB->Va_k = _rtB->Va_f;
14709
14710 /* Gain: '<S449>/Gain1' */
14711 _rtB->Gain1_m = 1000.0 * _rtB->thetaL;
14712
14713 /* DataTypeConversion: '<S449>/Data Type Conversion1' */
14714 TrigonometricFunction4_o = floor(_rtB->Gain1_m);
14715 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14716 {
14717 TrigonometricFunction4_o = 0.0;
14718 } else {
14719 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14720 }
14721
14722 _rtB->thetaL_e = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14723 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14724
14725 /* End of DataTypeConversion: '<S449>/Data Type Conversion1' */
14726 _rtB->thetaL_d = _rtB->thetaL_e;
14727
14728 /* Gain: '<S458>/Unit Conversion' */
14729 _rtB->Gain2_ij = 57.295779513082323 * _rtB->UnitConversion;
14730
14731 /* Gain: '<S449>/Gain2' */
14732 _rtB->Gain2_ij *= 1000.0;
14733
14734 /* DataTypeConversion: '<S449>/Data Type Conversion2' */
14735 TrigonometricFunction4_o = floor(_rtB->Gain2_ij);
14736 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14737 {
14738 TrigonometricFunction4_o = 0.0;
14739 } else {
14740 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14741 }
14742
14743 _rtB->psiL_j1 = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14744 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14745
14746 /* End of DataTypeConversion: '<S449>/Data Type Conversion2' */
14747 _rtB->psiL_j = _rtB->psiL_j1;
14748
14749 /* Gain: '<S449>/Gain3' */
14750 _rtB->Gain3_n = 1000.0 * _rtB->p;
14751
14752 /* DataTypeConversion: '<S449>/Data Type Conversion3' */
14753 TrigonometricFunction4_o = floor(_rtB->Gain3_n);
14754 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14755 {
14756 TrigonometricFunction4_o = 0.0;
14757 } else {
14758 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14759 }
14760
14761 _rtB->p_i = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14762 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14763
14764 /* End of DataTypeConversion: '<S449>/Data Type Conversion3' */
14765 _rtB->p_pm = _rtB->p_i;
14766
14767 /* Gain: '<S449>/Gain4' */
14768 _rtB->Gain4_e = 1000.0 * _rtB->q;
14769
14770 /* DataTypeConversion: '<S449>/Data Type Conversion4' */
14771 TrigonometricFunction4_o = floor(_rtB->Gain4_e);
14772 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14773 {
14774 TrigonometricFunction4_o = 0.0;
14775 } else {
14776 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14777 }
14778
14779 _rtB->q_i = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14780 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14781
14782 /* End of DataTypeConversion: '<S449>/Data Type Conversion4' */
14783 _rtB->q_je = _rtB->q_i;
14784
14785 /* Gain: '<S449>/Gain5' */
14786 _rtB->Gain5_l = 1000.0 * _rtB->r;
14787
14788 /* DataTypeConversion: '<S449>/Data Type Conversion5' */
14789 TrigonometricFunction4_o = floor(_rtB->Gain5_l);
14790 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14791 {
14792 TrigonometricFunction4_o = 0.0;
14793 } else {
14794 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14795 }
14796
14797 _rtB->r_a = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14798 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14799
14800 /* End of DataTypeConversion: '<S449>/Data Type Conversion5' */
14801 _rtB->r_l = _rtB->r_a;
14802
14803 /* Gain: '<S449>/Gain6' */
14804 _rtB->Gain6_l = 1000.0 * _rtB->Abx;
14805
14806 /* DataTypeConversion: '<S449>/Data Type Conversion6' */
14807 TrigonometricFunction4_o = floor(_rtB->Gain6_l);
14808 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14809 {
14810 TrigonometricFunction4_o = 0.0;
14811 } else {
14812 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14813 }
14814
14815 _rtB->Abx_e = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14816 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14817
14818 /* End of DataTypeConversion: '<S449>/Data Type Conversion6' */
14819 _rtB->Abx_o = _rtB->Abx_e;
14820
14821 /* Gain: '<S449>/Gain7' */
14822 _rtB->Gain7_n = 1000.0 * _rtB->Aby;
14823
14824 /* DataTypeConversion: '<S449>/Data Type Conversion7' */
14825 TrigonometricFunction4_o = floor(_rtB->Gain7_n);
14826 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14827 {
14828 TrigonometricFunction4_o = 0.0;
14829 } else {
14830 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14831 }
14832
14833 _rtB->Aby_m = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14834 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14835
14836 /* End of DataTypeConversion: '<S449>/Data Type Conversion7' */
14837 _rtB->Aby_a = _rtB->Aby_m;
14838
14839 /* Gain: '<S449>/Gain8' */
14840 _rtB->Gain8_g = 1000.0 * _rtB->Abz;
14841
14842 /* DataTypeConversion: '<S449>/Data Type Conversion8' */
14843 TrigonometricFunction4_o = floor(_rtB->Gain8_g);
14844 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
14845 {
14846 TrigonometricFunction4_o = 0.0;
14847 } else {
14848 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 4.294967296E+9);
14849 }
14850
14851 _rtB->Abz_a = TrigonometricFunction4_o < 0.0 ? -(int32_T)(uint32_T)
14852 -TrigonometricFunction4_o : (int32_T)(uint32_T)TrigonometricFunction4_o;
14853
14854 /* End of DataTypeConversion: '<S449>/Data Type Conversion8' */
14855 _rtB->Abz_h = _rtB->Abz_a;
14856 }
14857
14858 /* End of Switch: '<S445>/Switch' */
14859
14860 /* DataTypeConversion: '<S13>/Data Type Conversion24' */
14861 _rtB->Switch_dn = _rtB->Lon_m;
14862
14863 /* Gain: '<S13>/Gain16' */
14864 _rtB->Lon = 1.0E-7 * _rtB->Switch_dn;
14865
14866 /* DataTypeConversion: '<S13>/Data Type Conversion23' */
14867 _rtB->Switch_dn = _rtB->Lat_pm;
14868
14869 /* Gain: '<S13>/Gain15' */
14870 _rtB->Lat = 1.0E-7 * _rtB->Switch_dn;
14871
14872 /* DataTypeConversion: '<S13>/Data Type Conversion25' */
14873 _rtB->Switch_dn = _rtB->Alt_o3;
14874
14875 /* Gain: '<S13>/Gain17' */
14876 _rtB->Altitude = 0.001 * _rtB->Switch_dn;
14877
14878 /* MATLAB Function: '<S9>/MATLAB Function2' */
14879 _rtB->Alt_o = _rtB->Altitude;
14880
14881 /* MATLAB Function 'AFCS_MODEL1/PlanningSystem_LLA/MATLAB Function2': '<S334>:1' */
14882 /* '<S334>:1:16' */
14883 /* '<S334>:1:12' */
14884 /* '<S334>:1:1' */
14885 /* '<S334>:1:3' */
14886 /* '<S334>:1:4' */
14887 /* '<S334>:1:5' */
14888 /* '<S334>:1:7' */
14889 /* '<S334>:1:8' */
14890 /* '<S334>:1:9' */
14891 /* '<S334>:1:10' */
14892 /* '<S334>:1:11' */
14893 /* '<S334>:1:14' */
14894 /* '<S334>:1:17' */
14895 /* '<S334>:1:18' */
14896 /* '<S334>:1:19' */
14897 /* '<S334>:1:20' */
14898 /* '<S334>:1:21' */
14899 /* '<S334>:1:22' */
14900 /* '<S334>:1:23' */
14901 /* '<S334>:1:24' */
14902 /* '<S334>:1:25' */
14903 d_0 = -90.0 - _rtB->LLA0[0];
14904 AFCS_MODEL1_cosd_i(&d_0);
14905 e_0 = 90.0 + _rtB->LLA0[0];
14906 AFCS_MODEL1_sind_k(&e_0);
14907 f_0 = -90.0 - _rtB->LLA0[0];
14908 AFCS_MODEL1_sind_k(&f_0);
14909 g_0 = -90.0 - _rtB->LLA0[0];
14910 AFCS_MODEL1_cosd_i(&g_0);
14911 h_0 = _rtB->LLA0[1];
14912 AFCS_MODEL1_cosd_i(&h_0);
14913 i = _rtB->LLA0[1];
14914 AFCS_MODEL1_sind_k(&i);
14915 j_0 = -_rtB->LLA0[1];
14916 AFCS_MODEL1_sind_k(&j_0);
14917 k_0 = _rtB->LLA0[1];
14918 AFCS_MODEL1_cosd_i(&k_0);
14919 l_0 = _rtB->Lat;
14920 AFCS_MODEL1_cosd_i(&l_0);
14921 m_0 = _rtB->Lon;
14922 AFCS_MODEL1_cosd_i(&m_0);
14923 n = _rtB->LLA0[0];
14924 AFCS_MODEL1_cosd_i(&n);
14925 o = _rtB->LLA0[1];
14926 AFCS_MODEL1_cosd_i(&o);
14927 p = _rtB->Lat;
14928 AFCS_MODEL1_cosd_i(&p);
14929 q = _rtB->Lon;
14930 AFCS_MODEL1_sind_k(&q);
14931 TrigonometricFunction4_o = _rtB->LLA0[0];
14932 AFCS_MODEL1_cosd_i(&TrigonometricFunction4_o);
14933 T = _rtB->LLA0[1];
14934 AFCS_MODEL1_sind_k(&T);
14935 tmpForInput_idx_1 = _rtB->Lat;
14936 AFCS_MODEL1_sind_k(&tmpForInput_idx_1);
14937 tmpForInput_idx_2 = _rtB->LLA0[0];
14938 AFCS_MODEL1_sind_k(&tmpForInput_idx_2);
14939 d_2[0] = d_0;
14940 d_2[3] = 0.0;
14941 d_2[6] = e_0;
14942 d_2[1] = 0.0;
14943 d_2[4] = 1.0;
14944 d_2[7] = 0.0;
14945 d_2[2] = f_0;
14946 d_2[5] = 0.0;
14947 d_2[8] = g_0;
14948 h_2[0] = h_0;
14949 h_2[3] = i;
14950 h_2[6] = 0.0;
14951 h_2[1] = j_0;
14952 h_2[4] = k_0;
14953 h_2[7] = 0.0;
14954 h_2[2] = 0.0;
14955 h_2[5] = 0.0;
14956 h_2[8] = 1.0;
14957 for (i_1 = 0; i_1 < 3; i_1++) {
14958 for (flag = 0; flag < 3; flag++) {
14959 j_2[i_1 + 3 * flag] = 0.0;
14960 j_2[i_1 + 3 * flag] += h_2[3 * flag] * d_2[i_1];
14961 j_2[i_1 + 3 * flag] += h_2[3 * flag + 1] * d_2[i_1 + 3];
14962 j_2[i_1 + 3 * flag] += h_2[3 * flag + 2] * d_2[i_1 + 6];
14963 }
14964 }
14965
14966 tmpForInput_idx_0 = (6.378137E+6 + _rtB->Alt_o) * l_0 * m_0 - (6.378137E+6 +
14967 _rtB->LLA0[2]) * n * o;
14968 TrigonometricFunction4_o = (6.378137E+6 + _rtB->Alt_o) * p * q -
14969 (6.378137E+6 + _rtB->LLA0[2]) * TrigonometricFunction4_o * T;
14970 T = (6.378137E+6 + _rtB->Alt_o) * tmpForInput_idx_1 - (6.378137E+6 +
14971 _rtB->LLA0[2]) * tmpForInput_idx_2;
14972 for (i_1 = 0; i_1 < 3; i_1++) {
14973 Pos_NED[i_1] = j_2[i_1 + 6] * T + (j_2[i_1 + 3] * TrigonometricFunction4_o
14974 + j_2[i_1] * tmpForInput_idx_0);
14975 }
14976
14977 /* '<S334>:1:27' */
14978 _rtB->xn = Pos_NED[0];
14979
14980 /* '<S334>:1:28' */
14981 _rtB->ye = Pos_NED[1];
14982
14983 /* '<S334>:1:29' */
14984 _rtB->Alt_o -= _rtB->LLA0[2];
14985
14986 /* End of MATLAB Function: '<S9>/MATLAB Function2' */
14987
14988 /* Outport: '<Root>/X' */
14989 ((real_T *)ssGetOutputPortSignal(S, 0))[0] = _rtB->xn;
14990
14991 /* Outport: '<Root>/Y' */
14992 ((real_T *)ssGetOutputPortSignal(S, 1))[0] = _rtB->ye;
14993
14994 /* DataTypeConversion: '<S24>/Data Type Conversion45' */
14995 _rtB->altitude = (real32_T)_rtB->Altitude;
14996
14997 /* Outport: '<Root>/Hp' */
14998 ((real32_T *)ssGetOutputPortSignal(S, 2))[0] = _rtB->altitude;
14999
15000 /* Sum: '<S293>/Sum' */
15001 _rtB->mh_j = _rtB->altitude;
15002
15003 /* Outport: '<Root>/HBC' */
15004 ((real32_T *)ssGetOutputPortSignal(S, 3))[0] = _rtB->mh_j;
15005
15006 /* Outport: '<Root>/Hg' */
15007 ((real32_T *)ssGetOutputPortSignal(S, 4))[0] = _rtB->mh_j;
15008
15009 /* DataTypeConversion: '<S13>/Data Type Conversion26' */
15010 _rtB->Switch_dn = _rtB->Vn;
15011
15012 /* Gain: '<S13>/Gain12' */
15013 _rtB->GPS_vnorth = 0.001 * _rtB->Switch_dn;
15014
15015 /* DataTypeConversion: '<S24>/Data Type Conversion42' */
15016 _rtB->vnorth = (real32_T)_rtB->GPS_vnorth;
15017
15018 /* Outport: '<Root>/Vx' */
15019 ((real32_T *)ssGetOutputPortSignal(S, 5))[0] = _rtB->vnorth;
15020
15021 /* DataTypeConversion: '<S13>/Data Type Conversion27' */
15022 _rtB->Switch_dn = _rtB->Ve;
15023
15024 /* Gain: '<S13>/Gain13' */
15025 _rtB->GPS_veast = 0.001 * _rtB->Switch_dn;
15026
15027 /* DataTypeConversion: '<S24>/Data Type Conversion43' */
15028 _rtB->veast = (real32_T)_rtB->GPS_veast;
15029
15030 /* Outport: '<Root>/Vy' */
15031 ((real32_T *)ssGetOutputPortSignal(S, 6))[0] = _rtB->veast;
15032
15033 /* DataTypeConversion: '<S13>/Data Type Conversion28' */
15034 _rtB->Switch_dn = _rtB->Vd;
15035
15036 /* Gain: '<S13>/Gain14' */
15037 _rtB->GPS_vdown = 0.001 * _rtB->Switch_dn;
15038
15039 /* Gain: '<S24>/Gain4' */
15040 _rtB->Switch_dn = -_rtB->GPS_vdown;
15041
15042 /* DataTypeConversion: '<S24>/Data Type Conversion44' */
15043 _rtB->mhdot = (real32_T)_rtB->Switch_dn;
15044
15045 /* Outport: '<Root>/Vh' */
15046 ((real32_T *)ssGetOutputPortSignal(S, 7))[0] = _rtB->mhdot;
15047
15048 /* DataTypeConversion: '<S13>/Data Type Conversion30' */
15049 _rtB->Switch_dn = _rtB->Ndefine1;
15050
15051 /* Gain: '<S13>/Gain19' */
15052 _rtB->Switch_dn *= 0.001;
15053
15054 /* DataTypeConversion: '<S24>/Data Type Conversion9' */
15055 _rtB->Alfa_j = (real32_T)_rtB->Switch_dn;
15056
15057 /* Outport: '<Root>/Alpha' */
15058 ((real32_T *)ssGetOutputPortSignal(S, 8))[0] = _rtB->Alfa_j;
15059
15060 /* DataTypeConversion: '<S13>/Data Type Conversion31' */
15061 _rtB->Switch_dn = _rtB->Ndefine2;
15062
15063 /* Gain: '<S13>/Gain20' */
15064 _rtB->Switch_dn *= 0.001;
15065
15066 /* DataTypeConversion: '<S24>/Data Type Conversion10' */
15067 _rtB->Beta_f = (real32_T)_rtB->Switch_dn;
15068
15069 /* Outport: '<Root>/Beta' */
15070 ((real32_T *)ssGetOutputPortSignal(S, 9))[0] = _rtB->Beta_f;
15071
15072 /* DataTypeConversion: '<S13>/Data Type Conversion15' */
15073 _rtB->Switch_dn = _rtB->thetaL_d;
15074
15075 /* Gain: '<S13>/Gain3' */
15076 _rtB->Switch_dn *= 0.001;
15077
15078 /* DataTypeConversion: '<S24>/Data Type Conversion37' */
15079 _rtB->thetaL_k = (real32_T)_rtB->Switch_dn;
15080
15081 /* Outport: '<Root>/Theta' */
15082 ((real32_T *)ssGetOutputPortSignal(S, 10))[0] = _rtB->thetaL_k;
15083
15084 /* DataTypeConversion: '<S13>/Data Type Conversion14' */
15085 _rtB->Switch_dn = _rtB->phiL_j;
15086
15087 /* Gain: '<S13>/Gain9' */
15088 _rtB->Switch_dn *= 0.001;
15089
15090 /* DataTypeConversion: '<S24>/Data Type Conversion38' */
15091 _rtB->phiL_n = (real32_T)_rtB->Switch_dn;
15092
15093 /* Outport: '<Root>/Phi' */
15094 ((real32_T *)ssGetOutputPortSignal(S, 11))[0] = _rtB->phiL_n;
15095
15096 /* DataTypeConversion: '<S13>/Data Type Conversion16' */
15097 _rtB->Switch_dn = _rtB->psiL_j;
15098
15099 /* Gain: '<S13>/Gain4' */
15100 _rtB->Switch_dn *= 0.001;
15101
15102 /* DataTypeConversion: '<S24>/Data Type Conversion36' */
15103 _rtB->psiL_d = (real32_T)_rtB->Switch_dn;
15104
15105 /* Outport: '<Root>/Psi' */
15106 ((real32_T *)ssGetOutputPortSignal(S, 12))[0] = _rtB->psiL_d;
15107
15108 /* Trigonometry: '<S1>/Trigonometric Function2' */
15109 _rtB->Sqrt = AFCS_MODEL1_rt_atan2f_snf(_rtB->veast, _rtB->vnorth);
15110
15111 /* Gain: '<S3>/Unit Conversion' */
15112 _rtB->Sqrt *= 57.2957802F;
15113
15114 /* Outport: '<Root>/TRK' */
15115 ((real32_T *)ssGetOutputPortSignal(S, 13))[0] = _rtB->Sqrt;
15116
15117 /* DataTypeConversion: '<S13>/Data Type Conversion17' */
15118 _rtB->Switch_dn = _rtB->p_pm;
15119
15120 /* Gain: '<S13>/Gain5' */
15121 _rtB->Switch_dn *= 0.001;
15122
15123 /* DataTypeConversion: '<S24>/Data Type Conversion33' */
15124 _rtB->p_c = (real32_T)_rtB->Switch_dn;
15125
15126 /* Outport: '<Root>/p' */
15127 ((real32_T *)ssGetOutputPortSignal(S, 14))[0] = _rtB->p_c;
15128
15129 /* DataTypeConversion: '<S13>/Data Type Conversion18' */
15130 _rtB->Switch_dn = _rtB->q_je;
15131
15132 /* Gain: '<S13>/Gain6' */
15133 _rtB->Switch_dn *= 0.001;
15134
15135 /* DataTypeConversion: '<S24>/Data Type Conversion34' */
15136 _rtB->q_k = (real32_T)_rtB->Switch_dn;
15137
15138 /* Outport: '<Root>/q' */
15139 ((real32_T *)ssGetOutputPortSignal(S, 15))[0] = _rtB->q_k;
15140
15141 /* DataTypeConversion: '<S13>/Data Type Conversion19' */
15142 _rtB->Switch_dn = _rtB->r_l;
15143
15144 /* Gain: '<S13>/Gain7' */
15145 _rtB->Switch_dn *= 0.001;
15146
15147 /* DataTypeConversion: '<S24>/Data Type Conversion35' */
15148 _rtB->r_n = (real32_T)_rtB->Switch_dn;
15149
15150 /* Outport: '<Root>/r' */
15151 ((real32_T *)ssGetOutputPortSignal(S, 16))[0] = _rtB->r_n;
15152
15153 /* DataTypeConversion: '<S13>/Data Type Conversion20' */
15154 _rtB->Switch_dn = _rtB->Abx_o;
15155
15156 /* Gain: '<S13>/Gain8' */
15157 _rtB->Switch_dn *= 0.001;
15158
15159 /* Gain: '<S24>/Gain' */
15160 _rtB->Switch_dn *= 0.1019367991845056;
15161
15162 /* DataTypeConversion: '<S24>/Data Type Conversion39' */
15163 _rtB->Nx = (real32_T)_rtB->Switch_dn;
15164
15165 /* Outport: '<Root>/Ax' */
15166 ((real32_T *)ssGetOutputPortSignal(S, 17))[0] = _rtB->Nx;
15167
15168 /* DataTypeConversion: '<S13>/Data Type Conversion21' */
15169 _rtB->Switch_dn = _rtB->Aby_a;
15170
15171 /* Gain: '<S13>/Gain10' */
15172 _rtB->Switch_dn *= 0.001;
15173
15174 /* Gain: '<S24>/Gain1' */
15175 _rtB->Switch_dn *= 0.1019367991845056;
15176
15177 /* DataTypeConversion: '<S24>/Data Type Conversion40' */
15178 _rtB->Ny = (real32_T)_rtB->Switch_dn;
15179
15180 /* Outport: '<Root>/Ay' */
15181 ((real32_T *)ssGetOutputPortSignal(S, 18))[0] = _rtB->Ny;
15182
15183 /* DataTypeConversion: '<S13>/Data Type Conversion22' */
15184 _rtB->Switch_dn = _rtB->Abz_h;
15185
15186 /* Gain: '<S13>/Gain11' */
15187 _rtB->Switch_dn *= 0.001;
15188
15189 /* Gain: '<S24>/Gain2' */
15190 _rtB->Switch_dn *= 0.1019367991845056;
15191
15192 /* DataTypeConversion: '<S24>/Data Type Conversion41' */
15193 _rtB->Nz = (real32_T)_rtB->Switch_dn;
15194
15195 /* Outport: '<Root>/Az' */
15196 ((real32_T *)ssGetOutputPortSignal(S, 19))[0] = _rtB->Nz;
15197
15198 /* Product: '<S24>/Product' */
15199 _rtB->Sqrt = _rtB->vnorth * _rtB->vnorth;
15200
15201 /* Product: '<S24>/Product1' */
15202 _rtB->ascmd_n = _rtB->veast * _rtB->veast;
15203
15204 /* Product: '<S24>/Product2' */
15205 _rtB->Gain_e = _rtB->mhdot * _rtB->mhdot;
15206
15207 /* Sum: '<S24>/Sum' */
15208 _rtB->Sqrt = (_rtB->Sqrt + _rtB->ascmd_n) + _rtB->Gain_e;
15209
15210 /* Sqrt: '<S24>/Sqrt' */
15211 _rtB->Sqrt = (real32_T)sqrt(_rtB->Sqrt);
15212
15213 /* DataTypeConversion: '<S24>/Data Type Conversion32' */
15214 _rtB->vt = _rtB->Sqrt;
15215 }
15216
15217 /* End of Outputs for SubSystem: '<S1>/Subsystem5' */
15218 if (ssIsSampleHit(S, 1, 0)) {
15219 /* UnitDelay: '<S24>/Unit Delay3' */
15220 _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 352))[0];
15221
15222 /* RelationalOperator: '<S24>/Relational Operator1' incorporates:
15223 * Constant: '<S24>/Constant1'
15224 */
15225 _rtB->RelationalOperator19 = (0 == _rtB->UnitDelay5_b);
15226
15227 /* RelationalOperator: '<S24>/Relational Operator3' incorporates:
15228 * Constant: '<S24>/Constant7'
15229 */
15230 _rtB->RelationalOperator12 = (_rtB->UnitDelay5_b == 1);
15231
15232 /* RelationalOperator: '<S24>/Relational Operator2' incorporates:
15233 * Constant: '<S24>/Constant2'
15234 */
15235 _rtB->RelationalOperator2 = (_rtB->UnitDelay5_b == 6);
15236
15237 /* Logic: '<S24>/Logical Operator' */
15238 _rtB->LogicalOperator = (_rtB->RelationalOperator19 ||
15239 _rtB->RelationalOperator12 || _rtB->RelationalOperator2);
15240
15241 /* DataTypeConversion: '<S17>/Data Type Conversion1' */
15242 memset(&_rtB->routeFG[0], 0, 30U * sizeof(int32_T));
15243 }
15244
15245 if (1) {
15246 /* RelationalOperator: '<S24>/Relational Operator' incorporates:
15247 * Constant: '<S24>/Constant3'
15248 */
15249 _rtB->LogicalOperator1_b = (_rtB->vt < 0.3);
15250
15251 /* Logic: '<S24>/Logical Operator1' */
15252 _rtB->LogicalOperator1_b = (_rtB->LogicalOperator &&
15253 _rtB->LogicalOperator1_b);
15254
15255 /* Step: '<S17>/Step1' */
15256 _rtB->Switch_dn = !(ssGetTaskTime(S,0) < 1.0);
15257
15258 /* Sum: '<S17>/Sum' incorporates:
15259 * Constant: '<S17>/Constant14'
15260 */
15261 _rtB->UnaryMinus = 1.0 - _rtB->Switch_dn;
15262
15263 /* DataTypeConversion: '<S17>/Data Type Conversion2' */
15264 TrigonometricFunction4_o = floor(_rtB->UnaryMinus);
15265 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
15266 {
15267 TrigonometricFunction4_o = 0.0;
15268 } else {
15269 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);
15270 }
15271
15272 _rtB->airLineUpdateFlag = (int8_T)(TrigonometricFunction4_o < 0.0 ? (int32_T)
15273 (int8_T)-(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)(int8_T)
15274 (uint8_T)TrigonometricFunction4_o);
15275
15276 /* End of DataTypeConversion: '<S17>/Data Type Conversion2' */
15277
15278 /* RelationalOperator: '<S301>/Compare' incorporates:
15279 * Constant: '<S301>/Constant'
15280 */
15281 _rtB->Compare_c = (_rtB->airLineUpdateFlag == 1);
15282 }
15283
15284 if (ssIsSampleHit(S, 1, 0)) {
15285 /* SignalConversion: '<S299>/HiddenBuf_InsertedFor_Enabled Subsystem_at_inport_3' */
15286 _rtB->HiddenBuf_InsertedFor_EnabledSu = _rtB->Compare_c;
15287
15288 /* Outputs for Enabled SubSystem: '<S299>/Enabled Subsystem' incorporates:
15289 * EnablePort: '<S302>/Enable'
15290 */
15291 if (ssIsMajorTimeStep(S)) {
15292 if (_rtB->HiddenBuf_InsertedFor_EnabledSu) {
15293 if (!((boolean_T *)ssGetDWork(S, 613))[0]) {
15294 /* Outputs for Enabled SubSystem: '<S618>/GenerateGroundFM' incorporates:
15295 * EnablePort: '<S651>/Enable'
15296 */
15297 if (ssGetTaskTime(S,1) != ssGetTStart(S)) {
15298 ssSetSolverNeedsReset(S);
15299 }
15300
15301 /* End of Outputs for SubSystem: '<S618>/GenerateGroundFM' */
15302 ((boolean_T *)ssGetDWork(S, 613))[0] = true;
15303 }
15304 } else {
15305 if (((boolean_T *)ssGetDWork(S, 613))[0]) {
15306 ssSetSolverNeedsReset(S);
15307 ((boolean_T *)ssGetDWork(S, 613))[0] = false;
15308 }
15309 }
15310 }
15311
15312 /* End of Outputs for SubSystem: '<S299>/Enabled Subsystem' */
15313
15314 /* UnitDelay: '<S7>/Unit Delay' */
15315 _rtB->UnitDelay = ((int8_T *)ssGetDWork(S, 353))[0];
15316 }
15317
15318 /* Outputs for Enabled SubSystem: '<S299>/Enabled Subsystem' incorporates:
15319 * EnablePort: '<S302>/Enable'
15320 */
15321 if (((boolean_T *)ssGetDWork(S, 613))[0] && 1) {
15322 /* Inport: '<S302>/airNumPoint' */
15323 _rtB->airNumPoint = _rtB->NumPoint;
15324
15325 /* MATLAB Function: '<S302>/MATLAB Function' */
15326 /* MATLAB Function 'AFCS_MODEL1/GNC/NAV/PointNAV/Subsystem/Enabled Subsystem/MATLAB Function': '<S303>:1' */
15327 /* '<S303>:1:5' */
15328 memset(&_rtB->A_b[0], 0, 120U * sizeof(real_T));
15329
15330 /* '<S303>:1:7' */
15331 for (rtAction = 1; rtAction <= _rtB->airNumPoint; rtAction++) {
15332 /* '<S303>:1:7' */
15333 /* '<S303>:1:9' */
15334 i_1 = rtAction - 1;
15335 _rtB->A_b[i_1] = _rtB->route_Lon[rtAction - 1];
15336 _rtB->A_b[30 + i_1] = _rtB->route_Lat[rtAction - 1];
15337 _rtB->A_b[60 + i_1] = _rtB->routeAlt[rtAction - 1];
15338 _rtB->A_b[90 + i_1] = _rtB->routeFG[rtAction - 1];
15339 }
15340
15341 /* End of MATLAB Function: '<S302>/MATLAB Function' */
15342
15343 /* SignalConversion: '<S302>/OutportBufferForA' */
15344 memcpy(&_rtB->OutportBufferForA[0], &_rtB->A_b[0], 120U * sizeof(real_T));
15345 }
15346
15347 /* End of Outputs for SubSystem: '<S299>/Enabled Subsystem' */
15348 if (ssIsSampleHit(S, 1, 0)) {
15349 /* UnitDelay: '<S7>/Unit Delay3' */
15350 _rtB->UnitDelay3 = ((int8_T *)ssGetDWork(S, 354))[0];
15351 }
15352
15353 if (1) {
15354 /* MATLAB Function: '<S293>/MATLAB Function3' */
15355 rtAction = _rtB->UnitDelay;
15356
15357 /* MATLAB Function 'AFCS_MODEL1/GNC/NAV/PointNAV/MATLAB Function3': '<S298>:1' */
15358 if (_rtB->UnitDelay <= 0) {
15359 /* '<S298>:1:3' */
15360 /* '<S298>:1:4' */
15361 rtAction = 1;
15362 }
15363
15364 /* '<S298>:1:9' */
15365 i_1 = rtAction + 1;
15366 if (i_1 > 127) {
15367 i_1 = 127;
15368 }
15369
15370 /* '<S298>:1:11' */
15371 /* '<S298>:1:12' */
15372 /* Vxd=int32(A(kk,4)); */
15373 /* '<S298>:1:16' */
15374 /* '<S298>:1:17' */
15375 /* '<S298>:1:18' */
15376 /* Vxd1=int32(A(kk1,4)); */
15377 /* '<S298>:1:20' */
15378 flag = _rtB->airNumPoint - 3;
15379 if (flag < -128) {
15380 flag = -128;
15381 }
15382
15383 _rtB->Vxd1 = flag;
15384
15385 /* 当k为倒数第4个航点时,就要切到进场模式了 */
15386 /* %%%%%%%%%%%%%%%%%%%%% */
15387 /* '<S298>:1:23' */
15388 /* '<S298>:1:24' */
15389 /* '<S298>:1:25' */
15390 /* '<S298>:1:26' */
15391 /* '<S298>:1:27' */
15392 AFCS_MODEL1_GetPsi(_rtB->OutportBufferForA[rtAction - 1],
15393 _rtB->OutportBufferForA[rtAction + 29],
15394 _rtB->OutportBufferForA[(int8_T)i_1 - 1],
15395 _rtB->OutportBufferForA[(int8_T)i_1 + 29], &DD0, &PSDref0);
15396
15397 /* '<S298>:1:28' */
15398 /* '<S298>:1:29' */
15399 /* '<S298>:1:30' */
15400 _rtB->Href = (real32_T)_rtB->OutportBufferForA[(int8_T)i_1 + 59];
15401
15402 /* %%%%%%%%%%%%%%%%%%%%% */
15403 /* '<S298>:1:32' */
15404 /* '<S298>:1:33' */
15405 /* '<S298>:1:34' */
15406 AFCS_MODEL1_GetPsi(_rtB->OutportBufferForA[rtAction - 1],
15407 _rtB->OutportBufferForA[rtAction + 29], _rtB->Lon,
15408 _rtB->Lat, &Dp0, &PSDp0);
15409
15410 /* '<S298>:1:35' */
15411 /* '<S298>:1:36' */
15412 /* '<S298>:1:37' */
15413 DXp = -(real32_T)Dp0 * (real32_T)cos((real32_T)PSDp0);
15414
15415 /* '<S298>:1:38' */
15416 DYp = -(real32_T)Dp0 * (real32_T)sin((real32_T)PSDp0);
15417
15418 /* '<S298>:1:40' */
15419 COSA = (real32_T)cos((real32_T)PSDref0);
15420
15421 /* '<S298>:1:41' */
15422 COSB = (real32_T)sin((real32_T)PSDref0);
15423
15424 /* '<S298>:1:42' */
15425 _rtB->DtoStart_b = -DXp * COSA - DYp * COSB;
15426
15427 /* '<S298>:1:43' */
15428 _rtB->dXac = (real32_T)DD0 - (-DXp * COSA - DYp * COSB);
15429
15430 /* '<S298>:1:44' */
15431 _rtB->dYac = -DYp * COSA + DXp * COSB;
15432
15433 /* '<S298>:1:45' */
15434 _rtB->vxrw_j = -_rtB->vnorth * (real32_T)cos((real32_T)PSDref0) -
15435 _rtB->veast * (real32_T)sin((real32_T)PSDref0);
15436
15437 /* '<S298>:1:46' */
15438 _rtB->vyrw_f = -_rtB->vnorth * (real32_T)sin((real32_T)PSDref0) +
15439 _rtB->veast * (real32_T)cos((real32_T)PSDref0);
15440
15441 /* '<S298>:1:48' */
15442 i_1 = _rtB->airNumPoint - 2;
15443 if (i_1 < -128) {
15444 i_1 = -128;
15445 }
15446
15447 _rtB->HrefAL = (real32_T)_rtB->OutportBufferForA[i_1 + 59];
15448
15449 /* %进场的参考高度 */
15450 /* '<S298>:1:49' */
15451 _rtB->PSDref_n = (real32_T)PSDref0 * 57.2957802F;
15452
15453 /* End of MATLAB Function: '<S293>/MATLAB Function3' */
15454
15455 /* MATLAB Function: '<S293>/MATLAB Function2' */
15456 /* MATLAB Function 'AFCS_MODEL1/GNC/NAV/PointNAV/MATLAB Function2': '<S297>:1' */
15457 /* persistent InitFg IndexAL0; */
15458 /* if isempty(InitFg) */
15459 /* InitFg=int8(0); */
15460 /* IndexAL0=int8(1); */
15461 /* end */
15462 /* if(InitFg==int8(0)) */
15463 /* for i=1:NumPoint */
15464 /* Vxd=int32(A(i,4)); */
15465 /* if(Vxd==int32(7000)) */
15466 /* IndexAL0=i; */
15467 /* end */
15468 /* end */
15469 /* InitFg=int8(1); */
15470 /* end */
15471 /* '<S297>:1:17' */
15472 i_1 = _rtB->airNumPoint - 2;
15473 if (i_1 < -128) {
15474 i_1 = -128;
15475 }
15476
15477 if (1 >= (int8_T)i_1) {
15478 rtAction = 1;
15479 } else {
15480 rtAction = (int8_T)i_1;
15481 }
15482
15483 /* '<S297>:1:18' */
15484 _rtB->LonN = _rtB->OutportBufferForA[rtAction - 1];
15485
15486 /* '<S297>:1:19' */
15487 _rtB->LatN = _rtB->OutportBufferForA[rtAction + 29];
15488
15489 /* '<S297>:1:20' */
15490 _rtB->LonN1 = _rtB->OutportBufferForA[rtAction];
15491
15492 /* '<S297>:1:21' */
15493 _rtB->LatN1 = _rtB->OutportBufferForA[30 + rtAction];
15494 _rtB->IndexAL_k = rtAction;
15495
15496 /* End of MATLAB Function: '<S293>/MATLAB Function2' */
15497
15498 /* MATLAB Function: '<S293>/MATLAB Function1' */
15499 /* MATLAB Function 'AFCS_MODEL1/GNC/NAV/PointNAV/MATLAB Function1': '<S296>:1' */
15500 /* %%%%%%%%%%%%%%%%%%%%% */
15501 /* '<S296>:1:6' */
15502 /* '<S296>:1:14' */
15503 /* '<S296>:1:15' */
15504 /* '<S296>:1:16' */
15505 /* '<S296>:1:17' */
15506 /* '<S296>:1:6' */
15507 _rtB->Xnorth_j = (_rtB->Lat - _rtB->LatN) * 0.017453292519943295 * 6.371E+6;
15508
15509 /* '<S296>:1:6' */
15510 _rtB->Yeast_m = cos((_rtB->LatN + _rtB->Lat) * 0.5 * 0.017453292519943295) *
15511 ((_rtB->Lon - _rtB->LonN) * 0.017453292519943295) * 6.371E+6;
15512
15513 /* '<S296>:1:7' */
15514 /* '<S296>:1:14' */
15515 /* '<S296>:1:15' */
15516 /* '<S296>:1:16' */
15517 /* '<S296>:1:17' */
15518 /* '<S296>:1:7' */
15519 _rtB->XnorthD1 = (_rtB->LatN1 - _rtB->LatN) * 0.017453292519943295 *
15520 6.371E+6;
15521
15522 /* '<S296>:1:7' */
15523 _rtB->YeastD1 = cos((_rtB->LatN + _rtB->LatN1) * 0.5 * 0.017453292519943295)
15524 * ((_rtB->LonN1 - _rtB->LonN) * 0.017453292519943295) * 6.371E+6;
15525
15526 /* '<S296>:1:8' */
15527 _rtB->XnorthD = 0.0;
15528
15529 /* '<S296>:1:9' */
15530 _rtB->YeastD = 0.0;
15531
15532 /* DataTypeConversion: '<S293>/Data Type Conversion21' */
15533 _rtB->DataTypeConversion21 = (real32_T)_rtB->Xnorth_j;
15534
15535 /* DataTypeConversion: '<S293>/Data Type Conversion20' */
15536 _rtB->DataTypeConversion20 = (real32_T)_rtB->Yeast_m;
15537
15538 /* DataTypeConversion: '<S293>/Data Type Conversion9' */
15539 _rtB->DataTypeConversion9 = (real32_T)_rtB->XnorthD;
15540
15541 /* DataTypeConversion: '<S293>/Data Type Conversion12' */
15542 _rtB->DataTypeConversion12 = (real32_T)_rtB->YeastD;
15543
15544 /* DataTypeConversion: '<S293>/Data Type Conversion13' */
15545 _rtB->DataTypeConversion13 = (real32_T)_rtB->XnorthD1;
15546
15547 /* DataTypeConversion: '<S293>/Data Type Conversion14' */
15548 _rtB->DataTypeConversion14 = (real32_T)_rtB->YeastD1;
15549
15550 /* MATLAB Function: '<S293>/CompTP' incorporates:
15551 * Constant: '<S17>/Constant13'
15552 */
15553 /* MATLAB Function 'AFCS_MODEL1/GNC/NAV/PointNAV/CompTP': '<S295>:1' */
15554 /* Computing Tangent Points; */
15555 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算冻结航向 */
15556 if (_rtB->UnitDelay3 > ((int8_T *)ssGetDWork(S, 516))[0]) {
15557 /* '<S295>:1:16' */
15558 /* '<S295>:1:17' */
15559 ((real32_T *)ssGetDWork(S, 287))[0] = _rtB->DataTypeConversion21;
15560
15561 /* '<S295>:1:18' */
15562 ((real32_T *)ssGetDWork(S, 288))[0] = _rtB->DataTypeConversion20;
15563 }
15564
15565 /* '<S295>:1:20' */
15566 ((int8_T *)ssGetDWork(S, 516))[0] = _rtB->UnitDelay3;
15567
15568 /* %%%%%%%%%% */
15569 /* '<S295>:1:23' */
15570 AFCS_MODEL1_CompDistance(_rtB->DataTypeConversion9,
15571 _rtB->DataTypeConversion12, _rtB->DataTypeConversion13,
15572 _rtB->DataTypeConversion14, ((real32_T *)ssGetDWork(S, 287))[0],
15573 ((real32_T *)ssGetDWork(S, 288))[0], &DD, &PsdNorthD, &Xrw, &Yrw);
15574 if (Yrw < 0.0F) {
15575 /* '<S295>:1:24' */
15576 /* '<S295>:1:25' */
15577 DireFg = -1;
15578 } else {
15579 /* '<S295>:1:28' */
15580 DireFg = 1;
15581 }
15582
15583 /* %%%%%%%% */
15584 /* %%% */
15585 /* '<S295>:1:33' */
15586 b = PsdNorthD - 90.0F;
15587 AFCS_MODEL1_cosd(&b);
15588 Xcircl1 = 250.0F * b + _rtB->DataTypeConversion9;
15589
15590 /* '<S295>:1:34' */
15591 c = PsdNorthD - 90.0F;
15592 AFCS_MODEL1_sind(&c);
15593 Ycircl1 = 250.0F * c + _rtB->DataTypeConversion12;
15594
15595 /* %%% */
15596 /* '<S295>:1:36' */
15597 d_1 = PsdNorthD + 90.0F;
15598 AFCS_MODEL1_cosd(&d_1);
15599 Xcircl2 = 250.0F * d_1 + _rtB->DataTypeConversion9;
15600
15601 /* '<S295>:1:37' */
15602 e_1 = PsdNorthD + 90.0F;
15603 AFCS_MODEL1_sind(&e_1);
15604 Ycircl2 = 250.0F * e_1 + _rtB->DataTypeConversion12;
15605
15606 /* %%%%%%%%% */
15607 if (Yrw < 0.0F) {
15608 /* '<S295>:1:39' */
15609 /* '<S295>:1:40' */
15610 DX = ((real32_T *)ssGetDWork(S, 287))[0] - Xcircl1;
15611
15612 /* '<S295>:1:41' */
15613 DY = ((real32_T *)ssGetDWork(S, 288))[0] - Ycircl1;
15614
15615 /* '<S295>:1:42' */
15616 DDOC = (real32_T)sqrt(DX * DX + DY * DY);
15617
15618 /* '<S295>:1:43' */
15619 PsiOC = AFCS_MODEL1_rt_atan2f_snf(DY, DX) * 180.0F / 3.14159274F;
15620 } else {
15621 /* '<S295>:1:45' */
15622 DX = ((real32_T *)ssGetDWork(S, 287))[0] - Xcircl2;
15623
15624 /* '<S295>:1:46' */
15625 DY = ((real32_T *)ssGetDWork(S, 288))[0] - Ycircl2;
15626
15627 /* '<S295>:1:47' */
15628 DDOC = (real32_T)sqrt(DX * DX + DY * DY);
15629
15630 /* '<S295>:1:48' */
15631 PsiOC = AFCS_MODEL1_rt_atan2f_snf(DY, DX) * 180.0F / 3.14159274F;
15632 }
15633
15634 if (DDOC < 250.0F) {
15635 /* '<S295>:1:51' */
15636 /* %%%%%%%%% */
15637 if (Yrw < 0.0F) {
15638 /* '<S295>:1:53' */
15639 /* '<S295>:1:54' */
15640 DireFg = 1;
15641
15642 /* '<S295>:1:55' */
15643 DX = ((real32_T *)ssGetDWork(S, 287))[0] - Xcircl2;
15644
15645 /* '<S295>:1:56' */
15646 DY = ((real32_T *)ssGetDWork(S, 288))[0] - Ycircl2;
15647
15648 /* '<S295>:1:57' */
15649 /* '<S295>:1:58' */
15650 PsiOC = AFCS_MODEL1_rt_atan2f_snf(DY, DX) * 180.0F / 3.14159274F;
15651
15652 /* '<S295>:1:60' */
15653 DXp = 250.0F / (real32_T)sqrt(DX * DX + DY * DY);
15654 if (!(DXp <= 1.0F)) {
15655 DXp = 1.0F;
15656 }
15657
15658 Deta = 57.2957802F * (real32_T)acos(DXp);
15659
15660 /* %%右切点 */
15661 /* '<S295>:1:62' */
15662 alp1 = PsiOC + Deta;
15663
15664 /* %%左切点 */
15665 /* '<S295>:1:64' */
15666 alp2 = PsiOC - Deta;
15667
15668 /* '<S295>:1:65' */
15669 Xtp1 = alp1;
15670 AFCS_MODEL1_cosd(&Xtp1);
15671 Xtp1 = 250.0F * Xtp1 + Xcircl2;
15672
15673 /* '<S295>:1:66' */
15674 Ytp1 = alp1;
15675 AFCS_MODEL1_sind(&Ytp1);
15676 Ytp1 = 250.0F * Ytp1 + Ycircl2;
15677
15678 /* '<S295>:1:67' */
15679 Xtp2 = alp2;
15680 AFCS_MODEL1_cosd(&Xtp2);
15681 Xtp2 = 250.0F * Xtp2 + Xcircl2;
15682
15683 /* '<S295>:1:68' */
15684 DXp = alp2;
15685 AFCS_MODEL1_sind(&DXp);
15686 Ytp2 = 250.0F * DXp + Ycircl2;
15687 } else {
15688 /* '<S295>:1:72' */
15689 DireFg = -1;
15690
15691 /* '<S295>:1:73' */
15692 DX = ((real32_T *)ssGetDWork(S, 287))[0] - Xcircl1;
15693
15694 /* '<S295>:1:74' */
15695 DY = ((real32_T *)ssGetDWork(S, 288))[0] - Ycircl1;
15696
15697 /* '<S295>:1:75' */
15698 /* '<S295>:1:76' */
15699 PsiOC = AFCS_MODEL1_rt_atan2f_snf(DY, DX) * 180.0F / 3.14159274F;
15700
15701 /* '<S295>:1:78' */
15702 DXp = 250.0F / (real32_T)sqrt(DX * DX + DY * DY);
15703 if (!(DXp <= 1.0F)) {
15704 DXp = 1.0F;
15705 }
15706
15707 Deta = 57.2957802F * (real32_T)acos(DXp);
15708
15709 /* %%右切点 */
15710 /* '<S295>:1:80' */
15711 alp1 = PsiOC + Deta;
15712
15713 /* %%左切点 */
15714 /* '<S295>:1:82' */
15715 alp2 = PsiOC - Deta;
15716
15717 /* '<S295>:1:83' */
15718 Xtp1 = alp1;
15719 AFCS_MODEL1_cosd(&Xtp1);
15720 Xtp1 = 250.0F * Xtp1 + Xcircl1;
15721
15722 /* '<S295>:1:84' */
15723 Ytp1 = alp1;
15724 AFCS_MODEL1_sind(&Ytp1);
15725 Ytp1 = 250.0F * Ytp1 + Ycircl1;
15726
15727 /* '<S295>:1:85' */
15728 Xtp2 = alp2;
15729 AFCS_MODEL1_cosd(&Xtp2);
15730 Xtp2 = 250.0F * Xtp2 + Xcircl1;
15731
15732 /* '<S295>:1:86' */
15733 DXp = alp2;
15734 AFCS_MODEL1_sind(&DXp);
15735 Ytp2 = 250.0F * DXp + Ycircl1;
15736 }
15737 } else {
15738 /* %%%%%%%%% */
15739 /* %%%% */
15740 /* '<S295>:1:92' */
15741 DXp = 250.0F / DDOC;
15742 if (!(DXp <= 1.0F)) {
15743 DXp = 1.0F;
15744 }
15745
15746 Deta = 57.2957802F * (real32_T)acos(DXp);
15747
15748 /* %% */
15749 /* '<S295>:1:94' */
15750 alp1 = PsiOC + Deta;
15751
15752 /* %% */
15753 /* '<S295>:1:96' */
15754 alp2 = PsiOC - Deta;
15755
15756 /* %%%%% */
15757 if (Yrw < 0.0F) {
15758 /* '<S295>:1:99' */
15759 /* '<S295>:1:100' */
15760 g_1 = alp1;
15761 AFCS_MODEL1_cosd(&g_1);
15762 Xtp1 = 250.0F * g_1 + Xcircl1;
15763
15764 /* '<S295>:1:101' */
15765 i_0 = alp1;
15766 AFCS_MODEL1_sind(&i_0);
15767 Ytp1 = 250.0F * i_0 + Ycircl1;
15768
15769 /* '<S295>:1:102' */
15770 k_1 = alp2;
15771 AFCS_MODEL1_cosd(&k_1);
15772 Xtp2 = 250.0F * k_1 + Xcircl1;
15773
15774 /* '<S295>:1:103' */
15775 DXp = alp2;
15776 AFCS_MODEL1_sind(&DXp);
15777 Ytp2 = 250.0F * DXp + Ycircl1;
15778 } else {
15779 /* '<S295>:1:105' */
15780 f_1 = alp1;
15781 AFCS_MODEL1_cosd(&f_1);
15782 Xtp1 = 250.0F * f_1 + Xcircl2;
15783
15784 /* '<S295>:1:106' */
15785 h_1 = alp1;
15786 AFCS_MODEL1_sind(&h_1);
15787 Ytp1 = 250.0F * h_1 + Ycircl2;
15788
15789 /* '<S295>:1:107' */
15790 j_1 = alp2;
15791 AFCS_MODEL1_cosd(&j_1);
15792 Xtp2 = 250.0F * j_1 + Xcircl2;
15793
15794 /* '<S295>:1:108' */
15795 DXp = alp2;
15796 AFCS_MODEL1_sind(&DXp);
15797 Ytp2 = 250.0F * DXp + Ycircl2;
15798 }
15799 }
15800
15801 if (DireFg < 0) {
15802 /* '<S295>:1:112' */
15803 /* '<S295>:1:113' */
15804 XnorthTP = Xtp2;
15805
15806 /* '<S295>:1:114' */
15807 YeastTP = Ytp2;
15808 } else {
15809 /* '<S295>:1:116' */
15810 XnorthTP = Xtp1;
15811
15812 /* '<S295>:1:117' */
15813 YeastTP = Ytp1;
15814 }
15815
15816 /* '<S295>:1:120' */
15817 /* '<S295>:1:121' */
15818 /* '<S295>:1:122' */
15819 PsdNorthRef = AFCS_MODEL1_rt_atan2f_snf(YeastTP - ((real32_T *)ssGetDWork(S,
15820 288))[0], XnorthTP - ((real32_T *)ssGetDWork(S, 287))[0]) * 180.0F /
15821 3.14159274F;
15822
15823 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算相对于新航线的侧偏和待飞距 */
15824 /* '<S295>:1:125' */
15825 AFCS_MODEL1_CompDistance(((real32_T *)ssGetDWork(S, 287))[0], ((real32_T *)
15826 ssGetDWork(S, 288))[0], XnorthTP, YeastTP, _rtB->DataTypeConversion21,
15827 _rtB->DataTypeConversion20, &b_DD, &b_PsdNorthD, &b_Xrw, &b_Yrw);
15828
15829 /* '<S295>:1:125' */
15830 _rtB->Xrw = b_Xrw;
15831
15832 /* '<S295>:1:125' */
15833 _rtB->Yrw = b_Yrw;
15834
15835 /* '<S295>:1:126' */
15836 _rtB->vxrw_a = (real32_T)cos(PsdNorthRef * 3.14159274F / 180.0F) *
15837 -_rtB->vnorth - (real32_T)sin(PsdNorthRef * 3.14159274F / 180.0F) *
15838 _rtB->veast;
15839
15840 /* '<S295>:1:127' */
15841 _rtB->vyrw_n = (real32_T)sin(PsdNorthRef * 3.14159274F / 180.0F) *
15842 -_rtB->vnorth + (real32_T)cos(PsdNorthRef * 3.14159274F / 180.0F) *
15843 _rtB->veast;
15844
15845 /* %%%%%%计算当前位置与切入航点连线,与当前速度矢量夹角 */
15846 /* '<S295>:1:129' */
15847 /* '<S295>:1:130' */
15848 /* '<S295>:1:131' */
15849 /* '<S295>:1:132' */
15850 /* '<S295>:1:133' */
15851 _rtB->DetaPsi = AFCS_MODEL1_rt_atan2f_snf(_rtB->DataTypeConversion12 -
15852 _rtB->DataTypeConversion20, _rtB->DataTypeConversion9 -
15853 _rtB->DataTypeConversion21) * 180.0F / 3.14159274F -
15854 AFCS_MODEL1_rt_atan2f_snf(_rtB->veast, _rtB->vnorth) * 180.0F /
15855 3.14159274F;
15856 _rtB->PsdNorthRef = PsdNorthRef;
15857 _rtB->DireFg = (real32_T)DireFg;
15858
15859 /* End of MATLAB Function: '<S293>/CompTP' */
15860
15861 /* Switch: '<S293>/Switch' */
15862 if (_rtB->UnitDelay3 > 0) {
15863 _rtB->Switch_oz[0] = _rtB->HrefAL;
15864 _rtB->Switch_oz[1] = _rtB->Xrw;
15865 _rtB->Switch_oz[2] = _rtB->Yrw;
15866 _rtB->Switch_oz[3] = _rtB->PsdNorthRef;
15867 _rtB->Switch_oz[4] = _rtB->Xrw;
15868 _rtB->Switch_oz[5] = _rtB->vxrw_a;
15869 _rtB->Switch_oz[6] = _rtB->vyrw_n;
15870 } else {
15871 _rtB->Switch_oz[0] = _rtB->Href;
15872 _rtB->Switch_oz[1] = _rtB->dXac;
15873 _rtB->Switch_oz[2] = _rtB->dYac;
15874 _rtB->Switch_oz[3] = _rtB->PSDref_n;
15875 _rtB->Switch_oz[4] = _rtB->dXac;
15876 _rtB->Switch_oz[5] = _rtB->vxrw_j;
15877 _rtB->Switch_oz[6] = _rtB->vyrw_f;
15878 }
15879
15880 /* End of Switch: '<S293>/Switch' */
15881
15882 /* DataTypeConversion: '<S293>/Data Type Conversion6' */
15883 _rtB->vyrw = _rtB->Switch_oz[6];
15884
15885 /* DataTypeConversion: '<S293>/Data Type Conversion5' */
15886 _rtB->vxrw = _rtB->Switch_oz[5];
15887
15888 /* Gain: '<S24>/Gain5' */
15889 _rtB->Gain5_j = -_rtB->vxrw;
15890 }
15891
15892 if (ssIsSampleHit(S, 1, 0)) {
15893 /* Chart: '<S24>/zeros2' */
15894 /* Gateway: AFCS_MODEL1/GNC/NAV/zeros2 */
15895 /* During: AFCS_MODEL1/GNC/NAV/zeros2 */
15896 if (((uint8_T *)ssGetDWork(S, 586))[0] == 0U) {
15897 /* Entry: AFCS_MODEL1/GNC/NAV/zeros2 */
15898 ((uint8_T *)ssGetDWork(S, 586))[0] = 1U;
15899
15900 /* Entry Internal: AFCS_MODEL1/GNC/NAV/zeros2 */
15901 ((uint8_T *)ssGetDWork(S, 587))[0] = AFCS_MODEL1_IN_zero;
15902
15903 /* Entry 'zero': '<S294>:38' */
15904 _rtB->output = _rtB->Gain5_j + 1.0E-5F;
15905 } else {
15906 /* During 'zero': '<S294>:38' */
15907 /* Transition: '<S294>:151' */
15908 if ((_rtB->Gain5_j >= 0.0F) && (_rtB->Gain5_j < 1.0E-5F)) {
15909 /* Transition: '<S294>:149' */
15910 _rtB->output = 1.0E-5F;
15911 } else if ((_rtB->Gain5_j < 0.0F) && (_rtB->Gain5_j > -1.0E-5F)) {
15912 /* Transition: '<S294>:153' */
15913 _rtB->output = -1.0E-5F;
15914 } else {
15915 /* Transition: '<S294>:155' */
15916 _rtB->output = _rtB->Gain5_j;
15917 }
15918 }
15919
15920 /* End of Chart: '<S24>/zeros2' */
15921
15922 /* Saturate: '<S7>/Saturation1' incorporates:
15923 * Constant: '<S7>/Constant3'
15924 */
15925 _rtB->Saturation1_g = 0.005F;
15926 }
15927
15928 if (1) {
15929 /* Switch: '<S24>/Switch' incorporates:
15930 * Constant: '<S24>/Constant4'
15931 */
15932 if (_rtB->LogicalOperator1_b) {
15933 _rtB->UnaryMinus = 0.0;
15934 } else {
15935 /* Trigonometry: '<S24>/Trigonometric Function2' */
15936 _rtB->TrigonometricFunction2 = AFCS_MODEL1_rt_atan2f_snf(_rtB->vyrw,
15937 _rtB->output);
15938
15939 /* Gain: '<S283>/Unit Conversion' */
15940 _rtB->UnitConversion_a = 57.2957802F * _rtB->TrigonometricFunction2;
15941 _rtB->UnaryMinus = _rtB->UnitConversion_a;
15942 }
15943
15944 /* End of Switch: '<S24>/Switch' */
15945
15946 /* DataTypeConversion: '<S24>/Data Type Conversion8' */
15947 _rtB->psd = (real32_T)_rtB->UnaryMinus;
15948
15949 /* DataTypeConversion: '<S293>/Data Type Conversion1' */
15950 _rtB->yrw = _rtB->Switch_oz[2];
15951
15952 /* DataTypeConversion: '<S13>/Data Type Conversion32' */
15953 _rtB->UnaryMinus = _rtB->Ndefine3;
15954
15955 /* Gain: '<S13>/Gain21' */
15956 _rtB->UnaryMinus *= 0.001;
15957
15958 /* DataTypeConversion: '<S24>/Data Type Conversion2' */
15959 _rtB->Gain_e = (real32_T)_rtB->UnaryMinus;
15960
15961 /* Gain: '<S24>/Gain6' */
15962 _rtB->Qbar_d = 0.101936802F * _rtB->Gain_e;
15963
15964 /* DataTypeConversion: '<S24>/Data Type Conversion6' */
15965 _rtB->DataTypeConversion6 = _rtB->altitude;
15966
15967 /* MATLAB Function: '<S24>/Embedded MATLAB Function1' */
15968 /* MATLAB Function 'AFCS_MODEL1/GNC/NAV/Embedded MATLAB Function1': '<S292>:1' */
15969 /* %%%%%%% */
15970 /* '<S292>:1:3' */
15971 /* '<S292>:1:4' */
15972 TrigonometricFunction4_o = 6.371004E+6 * _rtB->DataTypeConversion6 /
15973 (6.371004E+6 + _rtB->DataTypeConversion6);
15974
15975 /* %%%%%% */
15976 if (_rtB->DataTypeConversion6 <= 11019.0) {
15977 /* '<S292>:1:6' */
15978 /* '<S292>:1:7' */
15979 _rtB->rou_f = AFCS_MODEL1_rt_powd_snf(1.0 - 2.2557700000000003E-5 *
15980 TrigonometricFunction4_o, 4.25588) * 1.225;
15981 } else if (_rtB->DataTypeConversion6 <= 20063.0) {
15982 /* '<S292>:1:8' */
15983 /* '<S292>:1:9' */
15984 _rtB->rou_f = exp((TrigonometricFunction4_o - 11000.0) *
15985 -0.00015768850000000002) * 0.3639176;
15986 } else if (_rtB->DataTypeConversion6 <= 32162.0) {
15987 /* '<S292>:1:10' */
15988 /* '<S292>:1:11' */
15989 _rtB->rou_f = AFCS_MODEL1_rt_powd_snf((TrigonometricFunction4_o - 20000.0)
15990 * 4.6157399999999991E-6 + 1.0, -35.16322) * 0.08803471;
15991 } else if (_rtB->DataTypeConversion6 <= 47350.0) {
15992 /* '<S292>:1:12' */
15993 /* '<S292>:1:13' */
15994 _rtB->rou_f = AFCS_MODEL1_rt_powd_snf((TrigonometricFunction4_o - 32000.0)
15995 * 1.224579E-5 + 1.0, -13.20115) * 0.01322497;
15996 } else if (_rtB->DataTypeConversion6 <= 51413.0) {
15997 /* '<S292>:1:14' */
15998 /* '<S292>:1:15' */
15999 _rtB->rou_f = exp((TrigonometricFunction4_o - 47000.0) *
16000 -0.00012622660000000002) * 0.001427527;
16001 } else if (_rtB->DataTypeConversion6 < 71802.0) {
16002 /* '<S292>:1:16' */
16003 /* '<S292>:1:17' */
16004 _rtB->rou_f = AFCS_MODEL1_rt_powd_snf(1.0 - (TrigonometricFunction4_o -
16005 51000.0) * 1.0345459999999998E-5, 11.20115) * 0.00086160110000000007;
16006 } else if (_rtB->DataTypeConversion6 < 86000.0) {
16007 /* '<S292>:1:18' */
16008 /* '<S292>:1:19' */
16009 _rtB->rou_f = AFCS_MODEL1_rt_powd_snf(1.0 - (TrigonometricFunction4_o -
16010 71000.0) * 9.3174939999999987E-6, 16.08161) * 6.421057E-5;
16011 } else {
16012 /* '<S292>:1:21' */
16013 _rtB->rou_f = exp((87284.8 - TrigonometricFunction4_o) / 5470.0) *
16014 4.4603475E-6;
16015 }
16016
16017 /* End of MATLAB Function: '<S24>/Embedded MATLAB Function1' */
16018
16019 /* Product: '<S24>/Divide1' */
16020 _rtB->UnaryMinus = _rtB->rou_f / 1.2250000238418579;
16021
16022 /* Sqrt: '<S24>/Sqrt1' */
16023 _rtB->UnaryMinus = sqrt(_rtB->UnaryMinus);
16024
16025 /* DataTypeConversion: '<S13>/Data Type Conversion29' */
16026 _rtB->Switch_dn = _rtB->Va_k;
16027
16028 /* Gain: '<S13>/Gain18' */
16029 _rtB->Switch_dn *= 0.001;
16030
16031 /* DataTypeConversion: '<S24>/Data Type Conversion1' */
16032 _rtB->Va_l = (real32_T)_rtB->Switch_dn;
16033
16034 /* Product: '<S24>/Product4' */
16035 _rtB->Switch_dn = _rtB->UnaryMinus * _rtB->Va_l;
16036
16037 /* DataTypeConversion: '<S24>/Data Type Conversion5' */
16038 _rtB->Vc_p = (real32_T)_rtB->Switch_dn;
16039
16040 /* Step: '<S1>/StartEngine1 ' */
16041 if (ssGetTaskTime(S,0) < 0.5) {
16042 _rtB->UnaryMinus = 0.0;
16043 } else {
16044 _rtB->UnaryMinus = 10.0;
16045 }
16046
16047 /* End of Step: '<S1>/StartEngine1 ' */
16048
16049 /* DataTypeConversion: '<S10>/Data Type Conversion29' */
16050 TrigonometricFunction4_o = floor(_rtB->UnaryMinus);
16051 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
16052 {
16053 TrigonometricFunction4_o = 0.0;
16054 } else {
16055 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);
16056 }
16057
16058 _rtB->StartEngine_qgc = (int8_T)(TrigonometricFunction4_o < 0.0 ? (int32_T)
16059 (int8_T)-(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)(int8_T)
16060 (uint8_T)TrigonometricFunction4_o);
16061
16062 /* End of DataTypeConversion: '<S10>/Data Type Conversion29' */
16063
16064 /* DataTypeConversion: '<S10>/Data Type Conversion1' */
16065 TrigonometricFunction4_o = floor(_rtB->u2u3_a);
16066 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
16067 {
16068 TrigonometricFunction4_o = 0.0;
16069 } else {
16070 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);
16071 }
16072
16073 _rtB->TakeoffFg_qgc = (int8_T)(TrigonometricFunction4_o < 0.0 ? (int32_T)
16074 (int8_T)-(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)(int8_T)
16075 (uint8_T)TrigonometricFunction4_o);
16076
16077 /* End of DataTypeConversion: '<S10>/Data Type Conversion1' */
16078
16079 /* Step: '<S10>/Step1' */
16080 _rtB->UnaryMinus = !(ssGetTaskTime(S,0) < 1.0);
16081
16082 /* DataTypeConversion: '<S10>/Data Type Conversion4' */
16083 TrigonometricFunction4_o = floor(_rtB->UnaryMinus);
16084 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
16085 {
16086 TrigonometricFunction4_o = 0.0;
16087 } else {
16088 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);
16089 }
16090
16091 _rtB->INS_switch = (int8_T)(TrigonometricFunction4_o < 0.0 ? (int32_T)
16092 (int8_T)-(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)(int8_T)
16093 (uint8_T)TrigonometricFunction4_o);
16094
16095 /* End of DataTypeConversion: '<S10>/Data Type Conversion4' */
16096
16097 /* Step: '<S10>/Step2' */
16098 _rtB->UnaryMinus = !(ssGetTaskTime(S,0) < 20.0);
16099
16100 /* DataTypeConversion: '<S10>/Data Type Conversion5' */
16101 TrigonometricFunction4_o = floor(_rtB->UnaryMinus);
16102 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
16103 {
16104 TrigonometricFunction4_o = 0.0;
16105 } else {
16106 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);
16107 }
16108
16109 _rtB->INSIntegralClear_Fg = (int8_T)(TrigonometricFunction4_o < 0.0 ?
16110 (int32_T)(int8_T)-(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)
16111 (int8_T)(uint8_T)TrigonometricFunction4_o);
16112
16113 /* End of DataTypeConversion: '<S10>/Data Type Conversion5' */
16114 }
16115
16116 if (ssIsSampleHit(S, 1, 0)) {
16117 /* Memory: '<S9>/Memory1' */
16118 _rtB->Memory1_px = ((boolean_T *)ssGetDWork(S, 612))[0];
16119
16120 /* Memory: '<S9>/Memory2' */
16121 _rtB->Memory2_o = ((int8_T *)ssGetDWork(S, 504))[0];
16122
16123 /* UnitDelay: '<S9>/Unit Delay' */
16124 _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 355))[0];
16125
16126 /* UnitDelay: '<S1>/Unit Delay1' */
16127 _rtB->UnitDelay1_l = ((int8_T *)ssGetDWork(S, 356))[0];
16128
16129 /* Logic: '<S9>/Logical Operator' */
16130 _rtB->LogicalOperator_j = (_rtB->UnitDelay5_b != 0) ^ (_rtB->UnitDelay1_l !=
16131 0);
16132
16133 /* Outputs for Atomic SubSystem: '<S9>/Trajectory planning,tracking' */
16134 /* UnitDelay: '<S423>/Output' */
16135 _rtB->Output = ((uint64_T *)ssGetDWork(S, 1))[0];
16136
16137 /* DataTypeConversion: '<S335>/Data Type Conversion' */
16138 _rtB->DataTypeConversion_g = (uint32_T)_rtB->Output;
16139
16140 /* End of Outputs for SubSystem: '<S9>/Trajectory planning,tracking' */
16141 }
16142
16143 /* Outputs for Atomic SubSystem: '<S9>/Trajectory planning,tracking' */
16144 if (1) {
16145 /* SignalConversion: '<S424>/TmpSignal ConversionAt SFunction Inport1' incorporates:
16146 * MATLAB Function: '<S335>/UAV Trajectory Planning'
16147 */
16148 _rtB->TmpSignalConversionAtSFunctionI[0] = _rtB->Lat;
16149 _rtB->TmpSignalConversionAtSFunctionI[1] = _rtB->Lon;
16150 _rtB->TmpSignalConversionAtSFunctionI[2] = _rtB->Altitude;
16151 _rtB->TmpSignalConversionAtSFunctionI[3] = _rtB->GPS_vnorth;
16152 _rtB->TmpSignalConversionAtSFunctionI[4] = _rtB->GPS_veast;
16153 _rtB->TmpSignalConversionAtSFunctionI[5] = _rtB->GPS_vdown;
16154
16155 /* MATLAB Function: '<S335>/UAV Trajectory Planning' */
16156 kk1 = _rtB->NumPoint;
16157
16158 /* MATLAB Function 'AFCS_MODEL1/PlanningSystem_LLA/Trajectory planning,tracking/UAV Trajectory Planning': '<S424>:1' */
16159 /* 轨迹规划模块:根据航点位置规划路径,路径由直线和圆弧组成 */
16160 /* 输入: */
16161 /* 1.UAVState,无人机当前位置、速度(pn,pe,pd,vn,ve) */
16162 /* 2.WayPoints,航路点及速度矩阵,数据格式为(x1..xn;y1...yn;z1...zn;v1...vn) */
16163 /* 3.Lengthofwp,航点个数 */
16164 /* 4.Fimax0,最大滚转角 */
16165 /* 5.flag_ChangePt,切点指令 */
16166 /* 6.TRACK_ToPt_cmd,指令目标点 */
16167 /* 7.Takeoff,起飞指令 */
16168 /* 输出:轨迹参数 */
16169 /* % */
16170 /* %航点个数 */
16171 /* 最多32个航点 */
16172 if (_rtB->NumPoint > 32) {
16173 /* '<S424>:1:16' */
16174 /* '<S424>:1:17' */
16175 kk1 = 32;
16176 }
16177
16178 /* % 航点个数 */
16179 /* % */
16180 /* 导航参数 */
16181 /* % */
16182 /* %当前目标航点 */
16183 /* 指令目标点 */
16184 /* 最小转弯半径 */
16185 /* % */
16186 /* *********************************************************************************% */
16187 /* '<S424>:1:36' */
16188 /* '<S424>:1:37' */
16189 /* % */
16190 /* Timestamp = t; */
16191 /* 北东下坐标系原点 */
16192 /* '<S424>:1:45' */
16193 Lat0 = _rtB->LLA0[0] * 0.017453292519943295;
16194
16195 /* '<S424>:1:46' */
16196 Lon0 = _rtB->LLA0[1] * 0.017453292519943295;
16197
16198 /* '<S424>:1:47' */
16199 /* '<S424>:1:48' */
16200 /* '<S424>:1:49' */
16201 /* '<S424>:1:50' */
16202 /* '<S424>:1:51' */
16203 AFCS_MODEL1_TransformLLATBDX(_rtB->TmpSignalConversionAtSFunctionI[1] *
16204 0.017453292519943295, _rtB->TmpSignalConversionAtSFunctionI[0] *
16205 0.017453292519943295, _rtB->TmpSignalConversionAtSFunctionI[2], Lon0, Lat0,
16206 _rtB->LLA0[2], &pn, &pe, &pd);
16207
16208 /* '<S424>:1:51' */
16209 /* '<S424>:1:52' */
16210 /* pn = UAVState(1); */
16211 /* pe = UAVState(2); */
16212 /* pd = UAVState(3); */
16213 /* '<S424>:1:57' */
16214 /* '<S424>:1:58' */
16215 /* '<S424>:1:59' */
16216 psi = AFCS_MODEL1_rt_atan2d_snf(_rtB->TmpSignalConversionAtSFunctionI[4],
16217 _rtB->TmpSignalConversionAtSFunctionI[3]);
16218
16219 /* psi是速度的偏角,相对于北向,顺时针是正,0~2pi */
16220 if (psi < 0.0) {
16221 /* '<S424>:1:60' */
16222 /* '<S424>:1:61' */
16223 psi += 6.2831853071795862;
16224 }
16225
16226 /* '<S424>:1:64' */
16227 /* 当前飞机的速度 */
16228 /* % */
16229 /* '<S424>:1:66' */
16230 AFCS_MODEL1_linspace(waypiont_xnAll);
16231
16232 /* linspace是均分计算指令,不知道为什么这么写,用zeros(1,IndataLengthMax)不好吗 */
16233 /* '<S424>:1:67' */
16234 AFCS_MODEL1_linspace(waypiont_yeAll);
16235
16236 /* 航点的y坐标 */
16237 /* '<S424>:1:68' */
16238 AFCS_MODEL1_linspace(waypiont_zhAll);
16239
16240 /* 航点的z坐标 */
16241 /* '<S424>:1:69' */
16242 waypiont_xnAll[0] = 0.0;
16243
16244 /* 人为添加的第一个航点,是起点,位置是0 */
16245 /* '<S424>:1:70' */
16246 waypiont_yeAll[0] = 0.0;
16247
16248 /* '<S424>:1:71' */
16249 waypiont_zhAll[0] = 0.0;
16250
16251 /* '<S424>:1:73' */
16252 AFCS_MODEL1_linspace(waypiont_velAll);
16253
16254 /* '<S424>:1:74' */
16255 waypiont_velAll[0] = _rtB->A[3];
16256
16257 /* 人为添加的第一个航点的速度,用航点文件里的第一个航点速度 */
16258 /* waypiont_velAll(1) = 20; */
16259 /* 将以经纬高形式的航路点改为北东地 */
16260 /* '<S424>:1:78' */
16261 i_1 = kk1 + 1;
16262 if (i_1 > 127) {
16263 i_1 = 127;
16264 }
16265
16266 /* '<S424>:1:78' */
16267 for (rtAction = 2; rtAction <= (int8_T)i_1; rtAction++) {
16268 /* '<S424>:1:78' */
16269 /* '<S424>:1:79' */
16270 flag = rtAction - 1;
16271 if (flag < -128) {
16272 flag = -128;
16273 }
16274
16275 tmp_c = rtAction - 1;
16276 if (tmp_c < -128) {
16277 tmp_c = -128;
16278 }
16279
16280 tmp_d = rtAction - 1;
16281 if (tmp_d < -128) {
16282 tmp_d = -128;
16283 }
16284
16285 AFCS_MODEL1_TransformLLATBDX(_rtB->A[(flag - 1) << 2] *
16286 0.017453292519943295, _rtB->A[1 + ((tmp_c - 1) << 2)] *
16287 0.017453292519943295, _rtB->A[2 + ((tmp_d - 1) << 2)], Lon0, Lat0,
16288 _rtB->LLA0[2], &b_0, &c_0, &unusedU0);
16289
16290 /* '<S424>:1:79' */
16291 waypiont_xnAll[rtAction - 1] = b_0;
16292
16293 /* '<S424>:1:79' */
16294 waypiont_yeAll[rtAction - 1] = c_0;
16295
16296 /* '<S424>:1:80' */
16297 flag = rtAction - 1;
16298 if (flag < -128) {
16299 flag = -128;
16300 }
16301
16302 waypiont_zhAll[rtAction - 1] = _rtB->A[((flag - 1) << 2) + 2];
16303
16304 /* 航点的相对高度 */
16305 /* '<S424>:1:81' */
16306 flag = rtAction - 1;
16307 if (flag < -128) {
16308 flag = -128;
16309 }
16310
16311 waypiont_velAll[rtAction - 1] = _rtB->A[((flag - 1) << 2) + 3];
16312
16313 /* 重新整理,实际航点比文件里的航点多了一个人为添加的起始原点 */
16314 /* waypiont_velAll(i) = 20; */
16315 }
16316
16317 /* % */
16318 /* '<S424>:1:87' */
16319 e_2 = 30.0;
16320 AFCS_MODEL1_tand(&e_2);
16321 B = 9.8 * e_2;
16322 for (i_1 = 0; i_1 < 32; i_1++) {
16323 RminAll[i_1] = waypiont_velAll[i_1] * waypiont_velAll[i_1] / B * 1.5;
16324 }
16325
16326 /* //最小转弯半径// */
16327 /* % */
16328 /* '<S424>:1:89' */
16329 i_1 = kk1 + 1;
16330 if (i_1 > 127) {
16331 i_1 = 127;
16332 }
16333
16334 /* 增点了原点航点,航点数增加1 */
16335 /* % */
16336 /* 起飞 */
16337 if (_rtB->LogicalOperator_j) {
16338 /* '<S424>:1:94' */
16339 /* 在起飞时/前,以飞机为航点0(消侧偏用,用于飞机不在航点0与航点1的连线上的情况) */
16340 /* 测试用,实际飞行待测 2020年5月17日16:52:44 */
16341 /* '<S424>:1:98' */
16342 waypiont_xnAll[1] = pn;
16343
16344 /* 把第二个航点设为飞机当前位置 */
16345 /* '<S424>:1:99' */
16346 waypiont_yeAll[1] = pe;
16347
16348 /* 飞起模式下,进来这,不在起飞模式,yValues(2)的值就变了 */
16349 /* '<S424>:1:100' */
16350 waypiont_zhAll[1] = -pd;
16351
16352 /* '<S424>:1:102' */
16353 AFCS_MODEL1_PGT_CalculateRmin(waypiont_xnAll, waypiont_yeAll, (int8_T)i_1,
16354 RminAll, RminAllNewCal);
16355
16356 /* 对Rmin进行微调,得到调整后所有航点的最小转弯半径 */
16357 /* '<S424>:1:103' */
16358 AFCS_MODEL1_PGT_changewaypoints(waypiont_xnAll, waypiont_yeAll,
16359 waypiont_zhAll, (int8_T)i_1, RminAllNewCal, waypiont_velAll, vx_qdout,
16360 vy_qdout, xn_qdout, ye_qdout, zh_qdout);
16361
16362 /* 切出点的速度和坐标 */
16363 /* '<S424>:1:105' */
16364 for (kk1 = 1; kk1 <= (int8_T)i_1; kk1++) {
16365 /* '<S424>:1:105' */
16366 /* '<S424>:1:106' */
16367 ((real_T *)ssGetDWork(S, 12))[kk1 - 1] = RminAllNewCal[kk1 - 1];
16368
16369 /* '<S424>:1:107' */
16370 ((real_T *)ssGetDWork(S, 16))[kk1 - 1] = vx_qdout[kk1 - 1];
16371
16372 /* 航点处的速度 */
16373 /* '<S424>:1:108' */
16374 ((real_T *)ssGetDWork(S, 17))[kk1 - 1] = vy_qdout[kk1 - 1];
16375
16376 /* '<S424>:1:109' */
16377 ((real_T *)ssGetDWork(S, 13))[kk1 - 1] = xn_qdout[kk1 - 1];
16378
16379 /* 航点处的切点的坐标 */
16380 /* '<S424>:1:110' */
16381 ((real_T *)ssGetDWork(S, 14))[kk1 - 1] = ye_qdout[kk1 - 1];
16382
16383 /* '<S424>:1:111' */
16384 ((real_T *)ssGetDWork(S, 15))[kk1 - 1] = zh_qdout[kk1 - 1];
16385 }
16386
16387 /* // 消除坐标原点对航线的影响 *****20190718 * ********** */
16388 /* '<S424>:1:114' */
16389 ((real_T *)ssGetDWork(S, 13))[1] = pn;
16390
16391 /* 第二个航点的切点还是第二个航点位置 */
16392 /* '<S424>:1:115' */
16393 ((real_T *)ssGetDWork(S, 14))[1] = pe;
16394
16395 /* //在有数据变动时,将当前飞机位置设置为转弯点,即从当前点开始调整航线 */
16396 /* //飞机当前转弯点为TRACK_ToPt+1 */
16397 /* 测试用,实际飞行待测 2020年5月17日16:52:44 */
16398 /* '<S424>:1:119' */
16399 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16400 if (qY > 65535U) {
16401 qY = 65535U;
16402 }
16403
16404 ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1] = pn;
16405
16406 /* '<S424>:1:120' */
16407 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16408 if (qY > 65535U) {
16409 qY = 65535U;
16410 }
16411
16412 ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1] = pe;
16413
16414 /* '<S424>:1:121' */
16415 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16416 if (qY > 65535U) {
16417 qY = 65535U;
16418 }
16419
16420 ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1] = -pd;
16421
16422 /* vx(TRACK_ToPt+1) = vn; */
16423 /* vy(TRACK_ToPt+1) = ve; */
16424 }
16425
16426 /* % */
16427 if (_rtB->Memory1_px) {
16428 /* '<S424>:1:126' */
16429 /* 看程序,在20s~20.02s时间段内,会变成1 */
16430 /* //在有数据变动时,将当前飞机位置设置为转弯点,即从当前点开始调整航线 */
16431 /* //飞机当前转弯点为TRACK_ToPt+1 */
16432 /* '<S424>:1:129' */
16433 rtAction = _rtB->Memory2_o;
16434 if (_rtB->Memory2_o < 0) {
16435 rtAction = 0;
16436 }
16437
16438 ((uint16_T *)ssGetDWork(S, 351))[0] = (uint16_T)rtAction;
16439
16440 /* '<S424>:1:130' */
16441 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16442 if (qY > 65535U) {
16443 qY = 65535U;
16444 }
16445
16446 ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1] = pn;
16447
16448 /* '<S424>:1:131' */
16449 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16450 if (qY > 65535U) {
16451 qY = 65535U;
16452 }
16453
16454 ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1] = pe;
16455
16456 /* '<S424>:1:132' */
16457 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16458 if (qY > 65535U) {
16459 qY = 65535U;
16460 }
16461
16462 ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1] = -pd;
16463
16464 /* '<S424>:1:133' */
16465 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16466 if (qY > 65535U) {
16467 qY = 65535U;
16468 }
16469
16470 ((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1] =
16471 _rtB->TmpSignalConversionAtSFunctionI[3];
16472
16473 /* '<S424>:1:134' */
16474 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16475 if (qY > 65535U) {
16476 qY = 65535U;
16477 }
16478
16479 ((real_T *)ssGetDWork(S, 17))[(int32_T)qY - 1] =
16480 _rtB->TmpSignalConversionAtSFunctionI[4];
16481
16482 /* if Vg > vValuesNew(TRACK_ToPt + 2) */
16483 /* RminNew(TRACK_ToPt + 2) = Vg * Vg / (9.8 * tand(Fimax)) * 1.3;%//以当前速度计算转弯半径 */
16484 /* else */
16485 /* RminNew(TRACK_ToPt + 2) = vValuesNew(TRACK_ToPt + 2)* vValuesNew(TRACK_ToPt + 2) / (9.8 * tand(Fimax)) * 1.3;%//以当前速度计算转弯半径 */
16486 /* end */
16487 }
16488
16489 /* % */
16490 /* '<S424>:1:143' */
16491 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16492 if (qY > 65535U) {
16493 qY = 65535U;
16494 }
16495
16496 postoNextqd_idx_0 = pn - ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1];
16497 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16498 if (qY > 65535U) {
16499 qY = 65535U;
16500 }
16501
16502 postoNextqd_idx_1 = pe - ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1];
16503
16504 /* 当前位置到下一航点的切点的向量 */
16505 /* '<S424>:1:144' */
16506 /* 相对位置点乘相对速度,是cos夹角 */
16507 /* '<S424>:1:146' */
16508 dclose = sqrt(postoNextqd_idx_0 * postoNextqd_idx_0 + postoNextqd_idx_1 *
16509 postoNextqd_idx_1);
16510
16511 /* 飞机当前位置距离下一航点的切点的距离 */
16512 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16513 if (qY > 65535U) {
16514 qY = 65535U;
16515 }
16516
16517 tmp_a = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16518 if (tmp_a > 65535U) {
16519 tmp_a = 65535U;
16520 }
16521
16522 tmp_b = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16523 if (tmp_b > 65535U) {
16524 tmp_b = 65535U;
16525 }
16526
16527 if (((((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1] * postoNextqd_idx_0 +
16528 ((real_T *)ssGetDWork(S, 17))[(int32_T)tmp_a - 1] * postoNextqd_idx_1 >
16529 0.0) && (dclose < ((real_T *)ssGetDWork(S, 12))[(int32_T)tmp_b - 1])) ||
16530 (dclose < sqrt(_rtB->TmpSignalConversionAtSFunctionI[3] *
16531 _rtB->TmpSignalConversionAtSFunctionI[3] +
16532 _rtB->TmpSignalConversionAtSFunctionI[4] *
16533 _rtB->TmpSignalConversionAtSFunctionI[4]) * 3.0)) {
16534 /* '<S424>:1:148' */
16535 /* 切航点的标志 */
16536 /* 切点,切点后需重新计算 */
16537 /* '<S424>:1:150' */
16538 AFCS_MODEL1_PGT_CalculateRmin(waypiont_xnAll, waypiont_yeAll, (int8_T)i_1,
16539 RminAll, RminAllNewCal);
16540
16541 /* 为什么要重新计算,这些值都没变 */
16542 /* '<S424>:1:151' */
16543 AFCS_MODEL1_PGT_changewaypoints(waypiont_xnAll, waypiont_yeAll,
16544 waypiont_zhAll, (int8_T)i_1, RminAllNewCal, waypiont_velAll, vx_qdout,
16545 vy_qdout, xn_qdout, ye_qdout, zh_qdout);
16546
16547 /* 航点的速度和航点的切点坐标 */
16548 /* '<S424>:1:152' */
16549 for (kk1 = 1; kk1 <= (int8_T)i_1; kk1++) {
16550 /* '<S424>:1:152' */
16551 /* '<S424>:1:153' */
16552 ((real_T *)ssGetDWork(S, 12))[kk1 - 1] = RminAllNewCal[kk1 - 1];
16553
16554 /* '<S424>:1:154' */
16555 ((real_T *)ssGetDWork(S, 16))[kk1 - 1] = vx_qdout[kk1 - 1];
16556
16557 /* '<S424>:1:155' */
16558 ((real_T *)ssGetDWork(S, 17))[kk1 - 1] = vy_qdout[kk1 - 1];
16559
16560 /* '<S424>:1:156' */
16561 ((real_T *)ssGetDWork(S, 13))[kk1 - 1] = xn_qdout[kk1 - 1];
16562
16563 /* '<S424>:1:157' */
16564 ((real_T *)ssGetDWork(S, 14))[kk1 - 1] = ye_qdout[kk1 - 1];
16565
16566 /* '<S424>:1:158' */
16567 ((real_T *)ssGetDWork(S, 15))[kk1 - 1] = zh_qdout[kk1 - 1];
16568 }
16569
16570 /* '<S424>:1:160' */
16571 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16572 if (qY > 65535U) {
16573 qY = 65535U;
16574 }
16575
16576 ((uint16_T *)ssGetDWork(S, 351))[0] = (uint16_T)qY;
16577
16578 /* 当前航点更新 */
16579 /* // 消除坐标原点对航线的影响 *****20190718 * ********** */
16580 /* '<S424>:1:162' */
16581 ((real_T *)ssGetDWork(S, 13))[1] = waypiont_xnAll[1];
16582
16583 /* '<S424>:1:163' */
16584 ((real_T *)ssGetDWork(S, 14))[1] = waypiont_yeAll[1];
16585 }
16586
16587 /* % */
16588 /* '<S424>:1:166' */
16589 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16590 if (qY > 65535U) {
16591 qY = 65535U;
16592 }
16593
16594 if ((int32_T)qY <= (int8_T)i_1) {
16595 /* '<S424>:1:168' */
16596 /* '<S424>:1:169' */
16597 /* 当前转弯点 */
16598 /* '<S424>:1:170' */
16599 /* '<S424>:1:171' */
16600 /* '<S424>:1:172' */
16601 /* '<S424>:1:173' */
16602 /* '<S424>:1:174' */
16603 /* 下一转弯点 */
16604 /* '<S424>:1:175' */
16605 /* '<S424>:1:176' */
16606 /* '<S424>:1:177' */
16607 /* '<S424>:1:178' */
16608 /* '<S424>:1:180' */
16609 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16610 if (qY > 65535U) {
16611 qY = 65535U;
16612 }
16613
16614 tmp_a = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16615 if (tmp_a > 65535U) {
16616 tmp_a = 65535U;
16617 }
16618
16619 tmp_b = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16620 if (tmp_b > 65535U) {
16621 tmp_b = 65535U;
16622 }
16623
16624 tmp_2 = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16625 if (tmp_2 > 65535U) {
16626 tmp_2 = 65535U;
16627 }
16628
16629 tmp_3 = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16630 if (tmp_3 > 65535U) {
16631 tmp_3 = 65535U;
16632 }
16633
16634 tmp_4 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16635 if (tmp_4 > 65535U) {
16636 tmp_4 = 65535U;
16637 }
16638
16639 tmp_5 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16640 if (tmp_5 > 65535U) {
16641 tmp_5 = 65535U;
16642 }
16643
16644 tmp_6 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16645 if (tmp_6 > 65535U) {
16646 tmp_6 = 65535U;
16647 }
16648
16649 tmp_7 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16650 if (tmp_7 > 65535U) {
16651 tmp_7 = 65535U;
16652 }
16653
16654 tmp_8 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16655 if (tmp_8 > 65535U) {
16656 tmp_8 = 65535U;
16657 }
16658
16659 tmp_9 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16660 if (tmp_9 > 65535U) {
16661 tmp_9 = 65535U;
16662 }
16663
16664 AFCS_MODE_PGT_Guidance3D_Dubins(((real_T *)ssGetDWork(S, 13))[(int32_T)qY
16665 - 1], ((real_T *)ssGetDWork(S, 14))[(int32_T)tmp_a - 1], ((real_T *)
16666 ssGetDWork(S, 15))[(int32_T)tmp_b - 1], ((real_T *)ssGetDWork(S, 16))
16667 [(int32_T)tmp_2 - 1], ((real_T *)ssGetDWork(S, 17))[(int32_T)tmp_3 - 1],
16668 ((real_T *)ssGetDWork(S, 13))[(int32_T)tmp_4 - 1], ((real_T *)ssGetDWork
16669 (S, 14))[(int32_T)tmp_5 - 1], ((real_T *)ssGetDWork(S, 15))[(int32_T)
16670 tmp_6 - 1], ((real_T *)ssGetDWork(S, 16))[(int32_T)tmp_7 - 1], ((real_T *)
16671 ssGetDWork(S, 17))[(int32_T)tmp_8 - 1], ((real_T *)ssGetDWork(S, 12))
16672 [(int32_T)tmp_9 - 1], &b_R, &unusedU1, &b_xq1, &b_yq1, &b_hq1, &b_fis,
16673 &b_Lamda1, &b_xq2, &b_yq2, &b_hq2, &b_fi2, &b_Lamda2, &b_x0_dot,
16674 &b_y0_dot, &b_tx_dot, &b_ty_dot, &b_fi1sum, &b_fi2sum);
16675
16676 /* '<S424>:1:182' */
16677 _rtB->TraParam[0] = b_R;
16678
16679 /* '<S424>:1:183' */
16680 _rtB->TraParam[1] = b_xq1;
16681
16682 /* 圆1的切出点坐标x */
16683 /* '<S424>:1:184' */
16684 _rtB->TraParam[2] = b_yq1;
16685
16686 /* 圆1的切出点坐标y */
16687 /* '<S424>:1:185' */
16688 _rtB->TraParam[3] = b_hq1;
16689
16690 /* 圆1的切出点高度h */
16691 /* '<S424>:1:186' */
16692 _rtB->TraParam[4] = b_fis;
16693
16694 /* 圆1的切出点角度 */
16695 /* '<S424>:1:187' */
16696 _rtB->TraParam[5] = b_Lamda1;
16697
16698 /* 圆1是顺时针还是逆时针,-1是顺时针,1是逆时针 */
16699 /* '<S424>:1:188' */
16700 _rtB->TraParam[6] = b_xq2;
16701
16702 /* 圆2的切入点坐标系x */
16703 /* '<S424>:1:189' */
16704 _rtB->TraParam[7] = b_yq2;
16705
16706 /* 圆2的切入点坐标系y */
16707 /* '<S424>:1:190' */
16708 _rtB->TraParam[8] = b_hq2;
16709
16710 /* 圆2的切入点高度h */
16711 /* '<S424>:1:191' */
16712 _rtB->TraParam[9] = b_fi2;
16713
16714 /* 圆2上的切入点角度 */
16715 /* '<S424>:1:192' */
16716 _rtB->TraParam[10] = b_Lamda2;
16717
16718 /* 圆2是顺时针还是逆时针 */
16719 /* '<S424>:1:193' */
16720 _rtB->TraParam[11] = b_x0_dot;
16721
16722 /* 圆1的圆心坐标x */
16723 /* '<S424>:1:194' */
16724 _rtB->TraParam[12] = b_y0_dot;
16725
16726 /* 圆1的圆心坐标y */
16727 /* '<S424>:1:195' */
16728 _rtB->TraParam[13] = b_tx_dot;
16729
16730 /* 圆2的圆心坐标x */
16731 /* '<S424>:1:196' */
16732 _rtB->TraParam[14] = b_ty_dot;
16733
16734 /* 圆2的圆心坐标y */
16735 /* '<S424>:1:197' */
16736 _rtB->TraParam[15] = b_fi1sum;
16737
16738 /* '<S424>:1:198' */
16739 _rtB->TraParam[16] = b_fi2sum;
16740
16741 /* '<S424>:1:199' */
16742 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16743 if (qY > 65535U) {
16744 qY = 65535U;
16745 }
16746
16747 _rtB->TraParam[17] = ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1];
16748
16749 /* 当前转弯点坐标x */
16750 /* '<S424>:1:200' */
16751 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16752 if (qY > 65535U) {
16753 qY = 65535U;
16754 }
16755
16756 _rtB->TraParam[18] = ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1];
16757
16758 /* 当前转弯点坐标y */
16759 /* '<S424>:1:201' */
16760 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16761 if (qY > 65535U) {
16762 qY = 65535U;
16763 }
16764
16765 _rtB->TraParam[19] = ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1];
16766
16767 /* 当前转弯点的高度 */
16768 /* '<S424>:1:202' */
16769 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16770 if (qY > 65535U) {
16771 qY = 65535U;
16772 }
16773
16774 _rtB->TraParam[20] = ((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1];
16775
16776 /* 当前转弯点速度vx */
16777 /* '<S424>:1:203' */
16778 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16779 if (qY > 65535U) {
16780 qY = 65535U;
16781 }
16782
16783 _rtB->TraParam[21] = ((real_T *)ssGetDWork(S, 17))[(int32_T)qY - 1];
16784
16785 /* 当前转弯点速度vy */
16786 /* '<S424>:1:204' */
16787 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16788 if (qY > 65535U) {
16789 qY = 65535U;
16790 }
16791
16792 _rtB->TraParam[22] = ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1];
16793
16794 /* 下一转弯点坐标x */
16795 /* '<S424>:1:205' */
16796 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16797 if (qY > 65535U) {
16798 qY = 65535U;
16799 }
16800
16801 _rtB->TraParam[23] = ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1];
16802
16803 /* 下一转弯点坐标y */
16804 /* '<S424>:1:206' */
16805 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16806 if (qY > 65535U) {
16807 qY = 65535U;
16808 }
16809
16810 _rtB->TraParam[24] = ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1];
16811
16812 /* 下一转弯高度 */
16813 /* '<S424>:1:207' */
16814 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16815 if (qY > 65535U) {
16816 qY = 65535U;
16817 }
16818
16819 _rtB->TraParam[25] = ((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1];
16820
16821 /* 下一转弯点速度vx */
16822 /* '<S424>:1:208' */
16823 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16824 if (qY > 65535U) {
16825 qY = 65535U;
16826 }
16827
16828 _rtB->TraParam[26] = ((real_T *)ssGetDWork(S, 17))[(int32_T)qY - 1];
16829
16830 /* 下一转弯点速度vy */
16831 /* '<S424>:1:209' */
16832 _rtB->TraParam[27] = pn;
16833
16834 /* 当前飞机位置x */
16835 /* '<S424>:1:210' */
16836 _rtB->TraParam[28] = pe;
16837
16838 /* 当前飞机位置y */
16839 /* '<S424>:1:211' */
16840 _rtB->TraParam[29] = -pd;
16841
16842 /* 当前飞机高度 */
16843 /* '<S424>:1:212' */
16844 _rtB->TraParam[30] = _rtB->TmpSignalConversionAtSFunctionI[3];
16845
16846 /* 当前飞机速度vx */
16847 /* '<S424>:1:213' */
16848 _rtB->TraParam[31] = _rtB->TmpSignalConversionAtSFunctionI[4];
16849
16850 /* 当前飞机速度vy */
16851 /* '<S424>:1:214' */
16852 _rtB->TraParam[32] = psi;
16853
16854 /* 当前飞机的速度偏角(相对于北向) */
16855 /* '<S424>:1:215' */
16856 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16857 if (qY > 65535U) {
16858 qY = 65535U;
16859 }
16860
16861 _rtB->TraParam[33] = waypiont_velAll[(int32_T)qY - 1];
16862
16863 /* 下一航点的速度 */
16864 qY = _rtB->DataTypeConversion_g - ((uint32_T *)ssGetDWork(S, 343))[0];
16865 if (qY > _rtB->DataTypeConversion_g) {
16866 qY = 0U;
16867 }
16868
16869 if (qY > 100U) {
16870 /* '<S424>:1:239' */
16871 /* 飞机的当前位置 */
16872 /* '<S424>:1:241' */
16873 ((uint32_T *)ssGetDWork(S, 343))[0] = _rtB->DataTypeConversion_g;
16874 }
16875 } else {
16876 /* '<S424>:1:246' */
16877 ((uint16_T *)ssGetDWork(S, 351))[0] = 0U;
16878
16879 /* '<S424>:1:247' */
16880 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16881 if (qY > 65535U) {
16882 qY = 65535U;
16883 }
16884
16885 ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1] = pn;
16886
16887 /* '<S424>:1:248' */
16888 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16889 if (qY > 65535U) {
16890 qY = 65535U;
16891 }
16892
16893 ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1] = pe;
16894
16895 /* '<S424>:1:249' */
16896 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16897 if (qY > 65535U) {
16898 qY = 65535U;
16899 }
16900
16901 ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1] = -pd;
16902
16903 /* '<S424>:1:250' */
16904 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16905 if (qY > 65535U) {
16906 qY = 65535U;
16907 }
16908
16909 ((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1] =
16910 _rtB->TmpSignalConversionAtSFunctionI[3];
16911
16912 /* '<S424>:1:251' */
16913 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16914 if (qY > 65535U) {
16915 qY = 65535U;
16916 }
16917
16918 ((real_T *)ssGetDWork(S, 17))[(int32_T)qY - 1] =
16919 _rtB->TmpSignalConversionAtSFunctionI[4];
16920
16921 /* '<S424>:1:253' */
16922 /* '<S424>:1:254' */
16923 /* '<S424>:1:255' */
16924 /* '<S424>:1:256' */
16925 /* '<S424>:1:257' */
16926 /* '<S424>:1:258' */
16927 /* '<S424>:1:259' */
16928 /* '<S424>:1:260' */
16929 /* '<S424>:1:261' */
16930 /* '<S424>:1:262' */
16931 /* '<S424>:1:264' */
16932 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16933 if (qY > 65535U) {
16934 qY = 65535U;
16935 }
16936
16937 tmp_a = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16938 if (tmp_a > 65535U) {
16939 tmp_a = 65535U;
16940 }
16941
16942 tmp_b = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16943 if (tmp_b > 65535U) {
16944 tmp_b = 65535U;
16945 }
16946
16947 tmp_2 = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16948 if (tmp_2 > 65535U) {
16949 tmp_2 = 65535U;
16950 }
16951
16952 tmp_3 = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
16953 if (tmp_3 > 65535U) {
16954 tmp_3 = 65535U;
16955 }
16956
16957 tmp_4 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16958 if (tmp_4 > 65535U) {
16959 tmp_4 = 65535U;
16960 }
16961
16962 tmp_5 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16963 if (tmp_5 > 65535U) {
16964 tmp_5 = 65535U;
16965 }
16966
16967 tmp_6 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16968 if (tmp_6 > 65535U) {
16969 tmp_6 = 65535U;
16970 }
16971
16972 tmp_7 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16973 if (tmp_7 > 65535U) {
16974 tmp_7 = 65535U;
16975 }
16976
16977 tmp_8 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16978 if (tmp_8 > 65535U) {
16979 tmp_8 = 65535U;
16980 }
16981
16982 tmp_9 = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
16983 if (tmp_9 > 65535U) {
16984 tmp_9 = 65535U;
16985 }
16986
16987 AFCS_MODE_PGT_Guidance3D_Dubins(((real_T *)ssGetDWork(S, 13))[(int32_T)qY
16988 - 1], ((real_T *)ssGetDWork(S, 14))[(int32_T)tmp_a - 1], ((real_T *)
16989 ssGetDWork(S, 15))[(int32_T)tmp_b - 1], ((real_T *)ssGetDWork(S, 16))
16990 [(int32_T)tmp_2 - 1], ((real_T *)ssGetDWork(S, 17))[(int32_T)tmp_3 - 1],
16991 ((real_T *)ssGetDWork(S, 13))[(int32_T)tmp_4 - 1], ((real_T *)ssGetDWork
16992 (S, 14))[(int32_T)tmp_5 - 1], ((real_T *)ssGetDWork(S, 15))[(int32_T)
16993 tmp_6 - 1], ((real_T *)ssGetDWork(S, 16))[(int32_T)tmp_7 - 1], ((real_T *)
16994 ssGetDWork(S, 17))[(int32_T)tmp_8 - 1], ((real_T *)ssGetDWork(S, 12))
16995 [(int32_T)tmp_9 - 1], &R, &unusedU2, &xq1, &yq1, &hq1, &fis, &Lamda1,
16996 &xq2, &yq2, &hq2, &fi2, &Lamda2, &x0_dot, &y0_dot, &tx_dot, &ty_dot,
16997 &fi1sum, &fi2sum);
16998
16999 /* '<S424>:1:266' */
17000 _rtB->TraParam[0] = R;
17001
17002 /* '<S424>:1:267' */
17003 _rtB->TraParam[1] = xq1;
17004
17005 /* '<S424>:1:268' */
17006 _rtB->TraParam[2] = yq1;
17007
17008 /* '<S424>:1:269' */
17009 _rtB->TraParam[3] = hq1;
17010
17011 /* '<S424>:1:270' */
17012 _rtB->TraParam[4] = fis;
17013
17014 /* '<S424>:1:271' */
17015 _rtB->TraParam[5] = Lamda1;
17016
17017 /* '<S424>:1:272' */
17018 _rtB->TraParam[6] = xq2;
17019
17020 /* '<S424>:1:273' */
17021 _rtB->TraParam[7] = yq2;
17022
17023 /* '<S424>:1:274' */
17024 _rtB->TraParam[8] = hq2;
17025
17026 /* '<S424>:1:275' */
17027 _rtB->TraParam[9] = fi2;
17028
17029 /* '<S424>:1:276' */
17030 _rtB->TraParam[10] = Lamda2;
17031
17032 /* '<S424>:1:277' */
17033 _rtB->TraParam[11] = x0_dot;
17034
17035 /* '<S424>:1:278' */
17036 _rtB->TraParam[12] = y0_dot;
17037
17038 /* '<S424>:1:279' */
17039 _rtB->TraParam[13] = tx_dot;
17040
17041 /* '<S424>:1:280' */
17042 _rtB->TraParam[14] = ty_dot;
17043
17044 /* '<S424>:1:281' */
17045 _rtB->TraParam[15] = fi1sum;
17046
17047 /* '<S424>:1:282' */
17048 _rtB->TraParam[16] = fi2sum;
17049
17050 /* '<S424>:1:283' */
17051 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
17052 if (qY > 65535U) {
17053 qY = 65535U;
17054 }
17055
17056 _rtB->TraParam[17] = ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1];
17057
17058 /* '<S424>:1:284' */
17059 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
17060 if (qY > 65535U) {
17061 qY = 65535U;
17062 }
17063
17064 _rtB->TraParam[18] = ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1];
17065
17066 /* '<S424>:1:285' */
17067 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
17068 if (qY > 65535U) {
17069 qY = 65535U;
17070 }
17071
17072 _rtB->TraParam[19] = ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1];
17073
17074 /* '<S424>:1:286' */
17075 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
17076 if (qY > 65535U) {
17077 qY = 65535U;
17078 }
17079
17080 _rtB->TraParam[20] = ((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1];
17081
17082 /* '<S424>:1:287' */
17083 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 1U;
17084 if (qY > 65535U) {
17085 qY = 65535U;
17086 }
17087
17088 _rtB->TraParam[21] = ((real_T *)ssGetDWork(S, 17))[(int32_T)qY - 1];
17089
17090 /* '<S424>:1:288' */
17091 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
17092 if (qY > 65535U) {
17093 qY = 65535U;
17094 }
17095
17096 _rtB->TraParam[22] = ((real_T *)ssGetDWork(S, 13))[(int32_T)qY - 1];
17097
17098 /* '<S424>:1:289' */
17099 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
17100 if (qY > 65535U) {
17101 qY = 65535U;
17102 }
17103
17104 _rtB->TraParam[23] = ((real_T *)ssGetDWork(S, 14))[(int32_T)qY - 1];
17105
17106 /* '<S424>:1:290' */
17107 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
17108 if (qY > 65535U) {
17109 qY = 65535U;
17110 }
17111
17112 _rtB->TraParam[24] = ((real_T *)ssGetDWork(S, 15))[(int32_T)qY - 1];
17113
17114 /* '<S424>:1:291' */
17115 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
17116 if (qY > 65535U) {
17117 qY = 65535U;
17118 }
17119
17120 _rtB->TraParam[25] = ((real_T *)ssGetDWork(S, 16))[(int32_T)qY - 1];
17121
17122 /* '<S424>:1:292' */
17123 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
17124 if (qY > 65535U) {
17125 qY = 65535U;
17126 }
17127
17128 _rtB->TraParam[26] = ((real_T *)ssGetDWork(S, 17))[(int32_T)qY - 1];
17129
17130 /* '<S424>:1:293' */
17131 _rtB->TraParam[27] = pn;
17132
17133 /* '<S424>:1:294' */
17134 _rtB->TraParam[28] = pe;
17135
17136 /* '<S424>:1:295' */
17137 _rtB->TraParam[29] = -pd;
17138
17139 /* '<S424>:1:296' */
17140 _rtB->TraParam[30] = _rtB->TmpSignalConversionAtSFunctionI[3];
17141
17142 /* '<S424>:1:297' */
17143 _rtB->TraParam[31] = _rtB->TmpSignalConversionAtSFunctionI[4];
17144
17145 /* '<S424>:1:298' */
17146 _rtB->TraParam[32] = psi;
17147
17148 /* '<S424>:1:299' */
17149 qY = ((uint16_T *)ssGetDWork(S, 351))[0] + 2U;
17150 if (qY > 65535U) {
17151 qY = 65535U;
17152 }
17153
17154 _rtB->TraParam[33] = waypiont_velAll[(int32_T)qY - 1];
17155 }
17156
17157 /* '<S424>:1:302' */
17158 _rtB->currentPoint = ((uint16_T *)ssGetDWork(S, 351))[0];
17159 _rtB->pn = pn;
17160 _rtB->pe = pe;
17161 _rtB->ph = -pd;
17162 }
17163
17164 if (ssIsSampleHit(S, 1, 0)) {
17165 /* SignalConversion: '<S335>/TmpSignal ConversionAtTo WorkspaceInport1' */
17166 _rtB->TmpSignalConversionAtToWorksp_o[0] = _rtB->pn;
17167 _rtB->TmpSignalConversionAtToWorksp_o[1] = _rtB->pe;
17168 _rtB->TmpSignalConversionAtToWorksp_o[2] = _rtB->ph;
17169
17170 /* Sum: '<S426>/FixPt Sum1' incorporates:
17171 * Constant: '<S426>/FixPt Constant'
17172 */
17173 _rtB->FixPtSum1 = _rtB->Output + 1ULL;
17174
17175 /* Switch: '<S427>/FixPt Switch' incorporates:
17176 * Constant: '<S427>/Constant'
17177 */
17178 if (_rtB->FixPtSum1 > 4294967296ULL) {
17179 _rtB->FixPtSwitch = 0ULL;
17180 } else {
17181 _rtB->FixPtSwitch = _rtB->FixPtSum1;
17182 }
17183
17184 /* End of Switch: '<S427>/FixPt Switch' */
17185 }
17186
17187 if (1) {
17188 /* MATLAB Function: '<S335>/UAV Trajectory Tracking' incorporates:
17189 * Constant: '<S9>/nav_param'
17190 */
17191 /* MATLAB Function 'AFCS_MODEL1/PlanningSystem_LLA/Trajectory planning,tracking/UAV Trajectory Tracking': '<S425>:1' */
17192 /* 轨迹跟踪模块:跟踪直线和圆弧轨迹 */
17193 /* 轨迹由圆弧、直线、圆弧组成,轨迹参数定义在输入参数里 */
17194 /* 输入: */
17195 /* 1.TraParam,跟踪轨迹参数 */
17196 /* 2.nav_param,导航参数 */
17197 /* 输出: */
17198 /* 1.hc,高度指令 */
17199 /* 2.psic,航向角指令 */
17200 /* 3.epy,侧偏距 */
17201 /* 4.Vc,速度大小指令 */
17202 /* 5.acmd,侧向加速度 */
17203 /* */
17204 /* '<S425>:1:16' */
17205 /* 半径 */
17206 /* '<S425>:1:17' */
17207 /* 圆1的切出点坐标x,有时,圆1的切出点和圆1的切点相同,有时有一些差别 */
17208 /* '<S425>:1:18' */
17209 /* 圆1的切出点坐标y */
17210 /* 圆1的切出点高度h */
17211 /* 圆1的切出点角度,与北方向的夹角 */
17212 /* '<S425>:1:21' */
17213 /* 圆1是顺时针还是逆时针,-1是顺时针,1是逆时针 */
17214 /* '<S425>:1:22' */
17215 /* 圆2的切入点坐标系x */
17216 /* '<S425>:1:23' */
17217 /* 圆2的切入点坐标系y */
17218 /* '<S425>:1:24' */
17219 /* 圆2的切入点高度h */
17220 /* 圆2上的切入点角度 */
17221 /* '<S425>:1:26' */
17222 /* 圆2是顺时针还是逆时针 */
17223 /* '<S425>:1:27' */
17224 /* 圆1的圆心坐标x */
17225 /* '<S425>:1:28' */
17226 /* 圆1的圆心坐标y */
17227 /* '<S425>:1:29' */
17228 /* 圆2的圆心坐标x */
17229 /* '<S425>:1:30' */
17230 /* 圆2的圆心坐标y */
17231 /* '<S425>:1:31' */
17232 /* '<S425>:1:32' */
17233 /* '<S425>:1:33' */
17234 /* 当前转弯点坐标x */
17235 /* '<S425>:1:34' */
17236 /* 当前转弯点坐标y */
17237 /* '<S425>:1:35' */
17238 /* 当前转弯点高度 */
17239 /* '<S425>:1:36' */
17240 /* 当前转弯点速度vx */
17241 /* '<S425>:1:37' */
17242 /* 当前转弯点速度vy */
17243 /* '<S425>:1:38' */
17244 /* 下一转弯点坐标x */
17245 /* '<S425>:1:39' */
17246 /* 下一转弯点坐标y */
17247 /* '<S425>:1:40' */
17248 /* 下一转弯点高度 */
17249 /* '<S425>:1:41' */
17250 /* 下一转弯点速度vx */
17251 /* '<S425>:1:42' */
17252 /* 下一转弯点速度vy */
17253 /* '<S425>:1:43' */
17254 /* 当前飞机位置x */
17255 /* '<S425>:1:44' */
17256 /* 当前飞机位置y */
17257 /* '<S425>:1:45' */
17258 /* 当前飞机高度 */
17259 /* '<S425>:1:46' */
17260 /* 当前飞机速度vx */
17261 /* '<S425>:1:47' */
17262 /* 当前飞机速度vy */
17263 /* '<S425>:1:48' */
17264 /* 当前飞机的速度偏角(相对于北向) */
17265 /* '<S425>:1:49' */
17266 _rtB->Vc = _rtB->TraParam[33];
17267
17268 /* 下一航点的速度 */
17269 /* 当前航点坐标x */
17270 /* 当前航点坐标y */
17271 /* 当前航点速度vx */
17272 /* 当前航点速度vy */
17273 /* 圆1的切出点坐标x */
17274 /* 圆1的切出点坐标y */
17275 /* '<S425>:1:58' */
17276 qsn = _rtB->TraParam[6] - _rtB->TraParam[1];
17277
17278 /* '<S425>:1:59' */
17279 qse = _rtB->TraParam[7] - _rtB->TraParam[2];
17280
17281 /* 圆2的切入点坐标系x */
17282 /* 圆2的切入点坐标系y */
17283 /* wen = pn; %这是原来的程序,这块应该写错了 */
17284 /* wee = pe; */
17285 /* qen = vn; */
17286 /* qee = ve; */
17287 /* 下一转弯点坐标x */
17288 /* 下一转弯点坐标y */
17289 /* 下一转弯点速度vx */
17290 /* 下一转弯点速度vy */
17291 /* '<S425>:1:72' */
17292 dR = 0.02 * _rtB->TraParam[0];
17293
17294 /* '<S425>:1:74' */
17295 _rtB->epy = 0.0;
17296
17297 /* % */
17298 /* 1 */
17299 /* gamac = Gama; */
17300 /* '<S425>:1:83' */
17301 /* '<S425>:1:91' */
17302 qn = _rtB->TraParam[6] - _rtB->TraParam[1];
17303
17304 /* 圆2的切入点坐标-圆1的切出点坐标 */
17305 /* '<S425>:1:92' */
17306 qe = _rtB->TraParam[7] - _rtB->TraParam[2];
17307
17308 /* '<S425>:1:97' */
17309 /* gamac = Gama; */
17310 /* '<S425>:1:100' */
17311 /* % */
17312 /* '<S425>:1:103' */
17313 dps = sqrt((_rtB->TraParam[27] - _rtB->TraParam[1]) * (_rtB->TraParam[27] -
17314 _rtB->TraParam[1]) + (_rtB->TraParam[28] - _rtB->TraParam[2]) *
17315 (_rtB->TraParam[28] - _rtB->TraParam[2]));
17316
17317 /* 飞机到圆1的切出点的距离 */
17318 /* '<S425>:1:104' */
17319 dpl = sqrt((_rtB->TraParam[27] - _rtB->TraParam[6]) * (_rtB->TraParam[27] -
17320 _rtB->TraParam[6]) + (_rtB->TraParam[28] - _rtB->TraParam[7]) *
17321 (_rtB->TraParam[28] - _rtB->TraParam[7]));
17322
17323 /* 飞机到圆2的切入点的距离 */
17324 /* % */
17325 if ((((_rtB->TraParam[27] - _rtB->TraParam[1]) * qsn + (_rtB->TraParam[28] -
17326 _rtB->TraParam[2]) * qse >= 0.0) && ((_rtB->TraParam[27] -
17327 _rtB->TraParam[6]) * qsn + (_rtB->TraParam[28] - _rtB->TraParam[7]) *
17328 qse <= 0.0)) || (dps < dR) || (dpl < dR)) {
17329 /* '<S425>:1:106' */
17330 /* 判断飞机跟踪直线的标志(参考说明文档) */
17331 /* '<S425>:1:107' */
17332 flag = 2;
17333
17334 /* 跟踪直线 */
17335 } else if (((_rtB->TraParam[27] - _rtB->TraParam[17]) * _rtB->TraParam[20] +
17336 (_rtB->TraParam[28] - _rtB->TraParam[18]) * _rtB->TraParam[21] >
17337 0.0) && ((_rtB->TraParam[27] - _rtB->TraParam[1]) * qsn +
17338 (_rtB->TraParam[28] - _rtB->TraParam[2]) * qse < 0.0))
17339 {
17340 /* '<S425>:1:108' */
17341 /* 判断飞机跟踪第一段圆弧的标志 */
17342 /* '<S425>:1:109' */
17343 flag = 1;
17344 } else if (((_rtB->TraParam[27] - _rtB->TraParam[6]) * qsn + (_rtB->
17345 TraParam[28] - _rtB->TraParam[7]) * qse > 0.0) &&
17346 ((_rtB->TraParam[27] - _rtB->TraParam[22]) * _rtB->TraParam[25] +
17347 (_rtB->TraParam[28] - _rtB->TraParam[23]) * _rtB->TraParam[26] <
17348 0.0)) {
17349 /* '<S425>:1:110' */
17350 /* 判断飞机跟踪第二段圆弧的标志 */
17351 /* '<S425>:1:111' */
17352 flag = 3;
17353 } else if ((_rtB->TraParam[27] - _rtB->TraParam[1]) * qsn + (_rtB->TraParam
17354 [28] - _rtB->TraParam[2]) * qse < 0.0) {
17355 /* '<S425>:1:112' */
17356 /* '<S425>:1:113' */
17357 flag = 1;
17358 } else if ((_rtB->TraParam[27] - _rtB->TraParam[6]) * qsn + (_rtB->TraParam
17359 [28] - _rtB->TraParam[7]) * qse > 0.0) {
17360 /* '<S425>:1:114' */
17361 /* '<S425>:1:115' */
17362 flag = 3;
17363 } else if ((_rtB->TraParam[27] - _rtB->TraParam[17]) * _rtB->TraParam[20] +
17364 (_rtB->TraParam[28] - _rtB->TraParam[18]) * _rtB->TraParam[21] >
17365 0.0) {
17366 /* '<S425>:1:116' */
17367 /* '<S425>:1:117' */
17368 flag = 1;
17369 } else {
17370 /* '<S425>:1:119' */
17371 flag = 3;
17372 }
17373
17374 if ((flag == 2) && (_rtB->TraParam[15] > 3.1415926535897931) &&
17375 (_rtB->TraParam[15] < 4.71238898038469) && ((_rtB->TraParam[27] -
17376 _rtB->TraParam[17]) * _rtB->TraParam[20] + (_rtB->TraParam[28] -
17377 _rtB->TraParam[18]) * _rtB->TraParam[21] >= 0.0)) {
17378 /* '<S425>:1:121' */
17379 /* '<S425>:1:122' */
17380 flag = 1;
17381 } else {
17382 if ((flag == 2) && (_rtB->TraParam[16] > 3.1415926535897931) &&
17383 ((_rtB->TraParam[27] - _rtB->TraParam[22]) * _rtB->TraParam[25] +
17384 (_rtB->TraParam[28] - _rtB->TraParam[23]) * _rtB->TraParam[26] < 0.0))
17385 {
17386 /* '<S425>:1:123' */
17387 /* '<S425>:1:124' */
17388 flag = 3;
17389 }
17390 }
17391
17392 /* % */
17393 /* %==============================平滑指令测试 2019年12月20日16:32:16===================================== */
17394 /* '<S425>:1:129' */
17395 Vg = sqrt(_rtB->TraParam[30] * _rtB->TraParam[30] + _rtB->TraParam[31] *
17396 _rtB->TraParam[31]);
17397 if ((flag == 1) && (dps < 4.0 * Vg)) {
17398 /* '<S425>:1:130' */
17399 /* 跟踪圆弧1时,飞机到圆弧1的切出点的距离太小,则直接跟踪直线 */
17400 /* '<S425>:1:131' */
17401 flag = 2;
17402 }
17403
17404 if ((flag == 2) && (dpl < 4.0 * Vg)) {
17405 /* '<S425>:1:134' */
17406 /* 跟踪直线时,飞机到圆弧2的切入点距离太小,则直接跟踪圆弧2 */
17407 /* '<S425>:1:135' */
17408 flag = 3;
17409 }
17410
17411 /* %==============================平滑指令测试 2019年12月20日16:32:16===================================== */
17412 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%2018/03/07%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
17413 /* % */
17414 if (flag == 1) {
17415 /* '<S425>:1:141' */
17416 /* '<S425>:1:142' */
17417 PGT_followOrbit_nonlinear_guida(_rtB->TraParam[11], _rtB->TraParam[12],
17418 _rtB->TraParam[0], -_rtB->TraParam[5], _rtB->TraParam[27],
17419 _rtB->TraParam[28], _rtB->TraParam[30], _rtB->TraParam[31],
17420 _rtB->TraParam[32], _rtB->TraParam[19], 4.0, &unusedU0_0, &b_fopsic,
17421 &b_acmd);
17422
17423 /* '<S425>:1:142' */
17424 _rtB->acmd = b_acmd;
17425
17426 /* 跟踪圆弧1 */
17427 /* '<S425>:1:143' */
17428 psic = b_fopsic;
17429 } else if (flag == 3) {
17430 /* '<S425>:1:144' */
17431 /* '<S425>:1:145' */
17432 PGT_followOrbit_nonlinear_guida(_rtB->TraParam[13], _rtB->TraParam[14],
17433 _rtB->TraParam[0], -_rtB->TraParam[10], _rtB->TraParam[27],
17434 _rtB->TraParam[28], _rtB->TraParam[30], _rtB->TraParam[31],
17435 _rtB->TraParam[32], -_rtB->TraParam[8], 4.0, &unusedU1_0, &fopsic, &acmd);
17436
17437 /* '<S425>:1:145' */
17438 _rtB->acmd = acmd;
17439
17440 /* 跟踪圆弧2 */
17441 /* '<S425>:1:146' */
17442 psic = fopsic;
17443 } else {
17444 /* '<S425>:1:148' */
17445 /* followstraightline 此处显示有关此函数的摘要 */
17446 /* Path definition r = (rn, re, rd)T , and q = (qn, qe, qd)T . MAV position p = (pn, pe, pd)T */
17447 /* course psi, gains psi_infinte, kpath. */
17448 /* 输入: */
17449 /* rn,re:圆1的切出点的坐标 */
17450 /* qn,qe:圆2的切入点相对于圆1的切出点的相对坐标 */
17451 /* pn,pe:飞机的当前位置 */
17452 /* vn,ve:飞机的速度 */
17453 /* psi:飞机的速度偏角(相对于北向) */
17454 b_Vg = sqrt(_rtB->TraParam[30] * _rtB->TraParam[30] + _rtB->TraParam[31] *
17455 _rtB->TraParam[31]);
17456 L1 = 4.0 * b_Vg;
17457
17458 /* L1 = 10*Vg; */
17459 /* L1 = 50; */
17460 Ru = sqrt((_rtB->TraParam[27] - _rtB->TraParam[1]) * (_rtB->TraParam[27] -
17461 _rtB->TraParam[1]) + (_rtB->TraParam[28] - _rtB->TraParam[2]) *
17462 (_rtB->TraParam[28] - _rtB->TraParam[2]));
17463 psiq = AFCS_MODEL1_rt_atan2d_snf(qe, qn);
17464
17465 /* if psiq < 0 */
17466 /* psiq = psiq + 2*pi; */
17467 /* end */
17468 /* if psiq-psi <= -pi */
17469 /* psiq = psiq + 2*pi; */
17470 /* end */
17471 /* if psiq-psi > pi */
17472 /* psiq = psiq - 2*pi; */
17473 /* end */
17474 epy = (_rtB->TraParam[27] - _rtB->TraParam[1]) * -sin(psiq) +
17475 (_rtB->TraParam[28] - _rtB->TraParam[2]) * cos(psiq);
17476 if (L1 * L1 - epy * epy > 0.0) {
17477 delt = sqrt(L1 * L1 - epy * epy);
17478 } else {
17479 /* delt = 50; */
17480 delt = 200.0;
17481 }
17482
17483 if (Ru * Ru - epy * epy > 0.0) {
17484 R_0 = sqrt(Ru * Ru - epy * epy);
17485 } else {
17486 R_0 = 100.0;
17487 }
17488
17489 /* ====================改进算法适用性 2019年12月20日17:32:44===================== */
17490 if ((_rtB->TraParam[27] - _rtB->TraParam[1]) * qn + (_rtB->TraParam[28] -
17491 _rtB->TraParam[2]) * qe > 0.0) {
17492 xt = (delt + R_0) * cos(psiq) + _rtB->TraParam[1];
17493 yt = (delt + R_0) * sin(psiq) + _rtB->TraParam[2];
17494 } else {
17495 xt = (delt - R_0) * cos(psiq) + _rtB->TraParam[1];
17496 yt = (delt - R_0) * sin(psiq) + _rtB->TraParam[2];
17497 }
17498
17499 /* ====================改进算法适用性 2019年12月20日17:32:44===================== */
17500 flpsic = AFCS_MODEL1_rt_atan2d_snf(yt - _rtB->TraParam[28], xt -
17501 _rtB->TraParam[27]);
17502 if (flpsic < 0.0) {
17503 flpsic += 6.2831853071795862;
17504 }
17505
17506 eta = flpsic - _rtB->TraParam[32];
17507 if (flpsic - _rtB->TraParam[32] < -3.1415926535897931) {
17508 flpsic += 6.2831853071795862;
17509 }
17510
17511 if (flpsic - _rtB->TraParam[32] > 3.1415926535897931) {
17512 flpsic -= 6.2831853071795862;
17513 }
17514
17515 /* aaa = 1; */
17516 /* 将相对关系画出来 */
17517 /* figure(1) */
17518 /* plot([re,re+qe],[rn,rn+qn],'LineStyle','--','Marker','o','LineWidth',2,'color','r'); %切入点和切出点的位置 */
17519 /* hold on */
17520 /* plot(pe,pn,'Marker','*','LineWidth',4);%飞机的位置 */
17521 /* plot(yt,xt,'Marker','o','color','b','LineWidth',4);%预瞄点的位置 */
17522 /* psi_deg = psi*57.3 */
17523 /* psic_deg = psic*57.3 */
17524 /* aa = 1; */
17525 /* psi_str = num2str(psi*57.3);psic_str = num2str(psic*57.3); */
17526 /* text(yt,xt,psic_str); */
17527 /* '<S425>:1:148' */
17528 _rtB->epy = epy;
17529
17530 /* '<S425>:1:148' */
17531 _rtB->acmd = 2.0 * b_Vg * b_Vg / sqrt((_rtB->TraParam[27] - xt) *
17532 (_rtB->TraParam[27] - xt) + (_rtB->TraParam[28] - yt) * (_rtB->TraParam
17533 [28] - yt)) * sin(eta);
17534
17535 /* 跟踪直线,只有跟踪直线有侧偏距 */
17536 /* '<S425>:1:149' */
17537 psic = flpsic;
17538
17539 /* close all */
17540 }
17541
17542 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%2018/03/07%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
17543 /* % */
17544 /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%2018/03/07%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
17545 /* psic = mod(psic, 2*pi); */
17546 if (psic < 0.0) {
17547 /* '<S425>:1:157' */
17548 /* '<S425>:1:158' */
17549 psic += 6.2831853071795862;
17550 }
17551
17552 /* '<S425>:1:160' */
17553 _rtB->hc = _rtB->TraParam[24];
17554
17555 /* nav_param = 1.5;%导航参数,根据实际情况修改 */
17556 /* psic =0.0; */
17557 /* hc = 0.0; */
17558 /* epy = 0.0; */
17559 /* acmd = 0.0; */
17560 /* [ psic,hc,epy,acmd] = PGT_follow3Ddubins( R,xq1,yq1,hq1,fis,Lamda1,xq2,yq2,hq2,fi2,Lamda2,x0_dot,y0_dot,tx_dot,ty_dot,fi1sum,fi2sum, xxs, yys, zzs, vxs, vys, xxf, yyf, zzf,vxf, vyf, pn, pe, pd, vn,ve,psi,nav_param); */
17561 _rtB->psic = psic;
17562
17563 /* End of MATLAB Function: '<S335>/UAV Trajectory Tracking' */
17564
17565 /* DataTypeConversion: '<S1>/Data Type Conversion3' */
17566 _rtB->Gain_e = (real32_T)_rtB->hc;
17567
17568 /* DataTypeConversion: '<S10>/Data Type Conversion14' */
17569 _rtB->ascmd_n = (real32_T)_rtB->acmd;
17570
17571 /* DataTypeConversion: '<S9>/Data Type Conversion38' */
17572 _rtB->StartEngine_qgc_k = (int8_T)_rtB->currentPoint;
17573
17574 /* BusCreator: '<S10>/Bus Creator1' */
17575 _rtB->BusCreator2.guidance.ytrack = 0.0F;
17576 _rtB->BusCreator2.guidance.altcmd = _rtB->Gain_e;
17577 _rtB->BusCreator2.guidance.headingcmd = 0.0F;
17578 _rtB->BusCreator2.guidance.ascmd = _rtB->ascmd_n;
17579 _rtB->BusCreator2.guidance.dtocenter = 0.0F;
17580 _rtB->BusCreator2.guidance.dtoTarget = 0.0F;
17581 _rtB->BusCreator2.guidance.currentPoint = 0;
17582 _rtB->BusCreator2.guidance.targetpoint = _rtB->StartEngine_qgc_k;
17583
17584 /* BusCreator: '<S10>/Bus Creator2' */
17585 _rtB->BusCreator2.decl_qgc = AFCS_MODEL1_rtC(S)->decl_qgc;
17586 _rtB->BusCreator2.dacl_qgc = AFCS_MODEL1_rtC(S)->dacl_qgc;
17587 _rtB->BusCreator2.drcl_qgc = AFCS_MODEL1_rtC(S)->drcl_qgc;
17588 _rtB->BusCreator2.nwscl_qgc = AFCS_MODEL1_rtC(S)->nwscl_qgc;
17589 _rtB->BusCreator2.sbcl_qgc = AFCS_MODEL1_rtC(S)->sbcl_qgc;
17590 _rtB->BusCreator2.StartEngine_qgc = _rtB->StartEngine_qgc;
17591 _rtB->BusCreator2.TakeoffFg_qgc = _rtB->TakeoffFg_qgc;
17592 _rtB->BusCreator2.Taxi_Fg = AFCS_MODEL1_rtC(S)->Taxi_Fg;
17593 _rtB->BusCreator2.LockFg_qgc = AFCS_MODEL1_rtC(S)->LockFg_qgc;
17594 _rtB->BusCreator2.INS_switch = _rtB->INS_switch;
17595 _rtB->BusCreator2.INSIntegralClear_Fg = _rtB->INSIntegralClear_Fg;
17596 _rtB->BusCreator2.EmergenReturnFg = 0;
17597 _rtB->BusCreator2.EmergenLandFg = 0;
17598 _rtB->BusCreator2.VcVtCutFg = 0;
17599 _rtB->BusCreator2.IniposCalFg = 0;
17600
17601 /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */
17602 _rtB->StartEngine_qgc_k = _rtB->BusCreator2.LockFg_qgc;
17603
17604 /* RelationalOperator: '<S42>/Compare' incorporates:
17605 * Constant: '<S42>/Constant'
17606 */
17607 _rtB->LogicalOperator4 = (_rtB->StartEngine_qgc_k == 10);
17608
17609 /* DataTypeConversion: '<S21>/Data Type Conversion8' */
17610 _rtB->LockUL = (int8_T)_rtB->LogicalOperator4;
17611
17612 /* RelationalOperator: '<S32>/Relational Operator' incorporates:
17613 * Constant: '<S32>/Constant2'
17614 */
17615 _rtB->LogicalOperator4 = (_rtB->LockUL == 0);
17616
17617 /* Switch: '<S32>/Switch' */
17618 _rtB->LockFlag = (int8_T)!_rtB->LogicalOperator4;
17619
17620 /* Step: '<S16>/Step3' */
17621 _rtB->UnaryMinus = !(ssGetTaskTime(S,0) < 80.0);
17622
17623 /* Step: '<S16>/Step5' */
17624 if (ssGetTaskTime(S,0) < 80.0) {
17625 _rtB->Switch_dn = 0.0;
17626 } else {
17627 _rtB->Switch_dn = -1.0;
17628 }
17629
17630 /* End of Step: '<S16>/Step5' */
17631
17632 /* Sum: '<S16>/Sum' */
17633 _rtB->UnaryMinus += _rtB->Switch_dn;
17634
17635 /* DataTypeConversion: '<S16>/Data Type Conversion7' */
17636 TrigonometricFunction4_o = floor(_rtB->UnaryMinus);
17637 if (rtIsNaN(TrigonometricFunction4_o) || rtIsInf(TrigonometricFunction4_o))
17638 {
17639 TrigonometricFunction4_o = 0.0;
17640 } else {
17641 TrigonometricFunction4_o = fmod(TrigonometricFunction4_o, 256.0);
17642 }
17643
17644 _rtB->StartEngine_qgc_k = (int8_T)(TrigonometricFunction4_o < 0.0 ? (int32_T)
17645 (int8_T)-(int8_T)(uint8_T)-TrigonometricFunction4_o : (int32_T)(int8_T)
17646 (uint8_T)TrigonometricFunction4_o);
17647
17648 /* End of DataTypeConversion: '<S16>/Data Type Conversion7' */
17649
17650 /* RelationalOperator: '<S36>/Compare' incorporates:
17651 * Constant: '<S36>/Constant'
17652 */
17653 _rtB->LogicalOperator4 = (_rtB->StartEngine_qgc_k >= 1);
17654
17655 /* DataTypeConversion: '<S21>/Data Type Conversion3' */
17656 _rtB->DataTypeConversion3_j = (int8_T)_rtB->LogicalOperator4;
17657 }
17658
17659 /* End of Outputs for SubSystem: '<S9>/Trajectory planning,tracking' */
17660 if (ssIsSampleHit(S, 1, 0)) {
17661 /* Chart: '<S21>/delayblock3' */
17662 /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock3 */
17663 /* During: AFCS_MODEL1/GNC/Calibration/delayblock3 */
17664 if (((uint8_T *)ssGetDWork(S, 603))[0] == 0U) {
17665 /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock3 */
17666 ((uint8_T *)ssGetDWork(S, 603))[0] = 1U;
17667
17668 /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock3 */
17669 ((uint8_T *)ssGetDWork(S, 604))[0] = AFCS_MODEL1_IN_delayblock;
17670
17671 /* Entry 'delayblock': '<S50>:38' */
17672 _rtB->Output_cw = 0;
17673
17674 /* Entry Internal 'delayblock': '<S50>:38' */
17675 /* Transition: '<S50>:164' */
17676 if (_rtB->DataTypeConversion3_j == 0) {
17677 /* Transition: '<S50>:160' */
17678 ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Input0;
17679
17680 /* Entry 'Input0': '<S50>:138' */
17681 ((int32_T *)ssGetDWork(S, 340))[0] = 0;
17682 } else {
17683 /* Transition: '<S50>:166' */
17684 ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Input1;
17685
17686 /* Entry 'Input1': '<S50>:165' */
17687 ((int32_T *)ssGetDWork(S, 340))[0] = 0;
17688 }
17689 } else {
17690 /* During 'delayblock': '<S50>:38' */
17691 switch (((uint8_T *)ssGetDWork(S, 605))[0]) {
17692 case AFCS_MODEL1_IN_Input0:
17693 /* During 'Input0': '<S50>:138' */
17694 if (((int32_T *)ssGetDWork(S, 340))[0] == 20) {
17695 /* Transition: '<S50>:137' */
17696 ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Output0;
17697
17698 /* Entry 'Output0': '<S50>:130' */
17699 _rtB->Output_cw = 0;
17700 ((int32_T *)ssGetDWork(S, 340))[0] = 0;
17701 } else if (_rtB->DataTypeConversion3_j == 1) {
17702 /* Transition: '<S50>:161' */
17703 ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Input1;
17704
17705 /* Entry 'Input1': '<S50>:165' */
17706 ((int32_T *)ssGetDWork(S, 340))[0] = 0;
17707 } else {
17708 ((int32_T *)ssGetDWork(S, 340))[0] = AFCS_MODEL1_addup_g(1, 20,
17709 ((int32_T *)ssGetDWork(S, 340))[0]);
17710 }
17711 break;
17712
17713 case AFCS_MODEL1_IN_Input1:
17714 /* During 'Input1': '<S50>:165' */
17715 if (((int32_T *)ssGetDWork(S, 340))[0] == 20) {
17716 /* Transition: '<S50>:168' */
17717 ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Output1;
17718
17719 /* Entry 'Output1': '<S50>:167' */
17720 _rtB->Output_cw = 1;
17721 ((int32_T *)ssGetDWork(S, 340))[0] = 0;
17722 } else if (_rtB->DataTypeConversion3_j == 0) {
17723 /* Transition: '<S50>:162' */
17724 ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Input0;
17725
17726 /* Entry 'Input0': '<S50>:138' */
17727 ((int32_T *)ssGetDWork(S, 340))[0] = 0;
17728 } else {
17729 ((int32_T *)ssGetDWork(S, 340))[0] = AFCS_MODEL1_addup_g(1, 20,
17730 ((int32_T *)ssGetDWork(S, 340))[0]);
17731 }
17732 break;
17733
17734 case AFCS_MODEL1_IN_Output0:
17735 /* During 'Output0': '<S50>:130' */
17736 if (_rtB->DataTypeConversion3_j == 1) {
17737 /* Transition: '<S50>:139' */
17738 ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Input1;
17739
17740 /* Entry 'Input1': '<S50>:165' */
17741 ((int32_T *)ssGetDWork(S, 340))[0] = 0;
17742 }
17743 break;
17744
17745 default:
17746 /* During 'Output1': '<S50>:167' */
17747 if (_rtB->DataTypeConversion3_j == 0) {
17748 /* Transition: '<S50>:170' */
17749 ((uint8_T *)ssGetDWork(S, 605))[0] = AFCS_MODEL1_IN_Input0;
17750
17751 /* Entry 'Input0': '<S50>:138' */
17752 ((int32_T *)ssGetDWork(S, 340))[0] = 0;
17753 }
17754 break;
17755 }
17756 }
17757
17758 /* End of Chart: '<S21>/delayblock3' */
17759
17760 /* Logic: '<S32>/Logical Operator' */
17761 _rtB->LogicalOperator_d = !(_rtB->Output_cw != 0);
17762
17763 /* UnitDelay: '<S31>/Unit Delay' */
17764 _rtB->UnitDelay_e = ((int8_T *)ssGetDWork(S, 357))[0];
17765
17766 /* UnitDelay: '<S31>/Unit Delay1' */
17767 _rtB->UnitDelay1_p = ((int8_T *)ssGetDWork(S, 358))[0];
17768
17769 /* UnitDelay: '<S31>/Unit Delay2' */
17770 _rtB->UnitDelay2 = ((int8_T *)ssGetDWork(S, 359))[0];
17771
17772 /* UnitDelay: '<S31>/Unit Delay3' */
17773 _rtB->UnitDelay3_l = ((int8_T *)ssGetDWork(S, 360))[0];
17774
17775 /* UnitDelay: '<S31>/Unit Delay4' */
17776 _rtB->UnitDelay4 = ((int8_T *)ssGetDWork(S, 361))[0];
17777
17778 /* UnitDelay: '<S31>/Unit Delay7' */
17779 _rtB->UnitDelay7 = ((int8_T *)ssGetDWork(S, 362))[0];
17780
17781 /* UnitDelay: '<S31>/Unit Delay5' */
17782 _rtB->UnitDelay5 = ((int8_T *)ssGetDWork(S, 363))[0];
17783
17784 /* MATLAB Function: '<S31>/LogicTrans' */
17785 /* MATLAB Function 'AFCS_MODEL1/GNC/logic/LogicTrans': '<S317>:1' */
17786 /* '<S317>:1:8' */
17787 switch (((int8_T *)ssGetDWork(S, 514))[0]) {
17788 case 0:
17789 /* '<S317>:1:10' */
17790 _rtB->fmphase = 0;
17791 if (_rtB->UnitDelay_e == 1) {
17792 /* '<S317>:1:11' */
17793 /* '<S317>:1:12' */
17794 _rtB->fmphase = 1;
17795 }
17796 break;
17797
17798 case 1:
17799 /* '<S317>:1:15' */
17800 _rtB->fmphase = 1;
17801 if (_rtB->UnitDelay1_p == 1) {
17802 /* '<S317>:1:16' */
17803 /* '<S317>:1:17' */
17804 _rtB->fmphase = 2;
17805 }
17806
17807 if (_rtB->UnitDelay7 == 1) {
17808 /* '<S317>:1:19' */
17809 /* '<S317>:1:20' */
17810 _rtB->fmphase = 6;
17811 }
17812 break;
17813
17814 case 2:
17815 /* '<S317>:1:23' */
17816 _rtB->fmphase = 2;
17817 if (_rtB->UnitDelay2 == 1) {
17818 /* '<S317>:1:24' */
17819 /* '<S317>:1:25' */
17820 _rtB->fmphase = 3;
17821 }
17822 break;
17823
17824 case 3:
17825 /* '<S317>:1:28' */
17826 _rtB->fmphase = 3;
17827 if (_rtB->UnitDelay3_l == 1) {
17828 /* '<S317>:1:29' */
17829 /* '<S317>:1:30' */
17830 _rtB->fmphase = 4;
17831 }
17832 break;
17833
17834 case 4:
17835 /* '<S317>:1:33' */
17836 _rtB->fmphase = 4;
17837 if (_rtB->UnitDelay4 == 1) {
17838 /* '<S317>:1:34' */
17839 /* '<S317>:1:35' */
17840 _rtB->fmphase = 5;
17841 }
17842 break;
17843
17844 case 5:
17845 /* '<S317>:1:38' */
17846 _rtB->fmphase = 5;
17847 if (_rtB->UnitDelay5 == 1) {
17848 /* '<S317>:1:39' */
17849 /* '<S317>:1:40' */
17850 _rtB->fmphase = 6;
17851 }
17852 break;
17853
17854 case 6:
17855 /* '<S317>:1:44' */
17856 _rtB->fmphase = 6;
17857 break;
17858
17859 default:
17860 /* '<S317>:1:47' */
17861 _rtB->fmphase = 0;
17862 break;
17863 }
17864
17865 /* '<S317>:1:50' */
17866 ((int8_T *)ssGetDWork(S, 514))[0] = _rtB->fmphase;
17867
17868 /* End of MATLAB Function: '<S31>/LogicTrans' */
17869
17870 /* RelationalOperator: '<S32>/Relational Operator1' incorporates:
17871 * Constant: '<S32>/Constant3'
17872 */
17873 _rtB->RelationalOperator1 = (_rtB->fmphase == 0);
17874 }
17875
17876 if (1) {
17877 /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */
17878 _rtB->StartEngine_qgc_k = _rtB->BusCreator2.StartEngine_qgc;
17879
17880 /* RelationalOperator: '<S40>/Compare' incorporates:
17881 * Constant: '<S40>/Constant'
17882 */
17883 _rtB->LogicalOperator4 = (_rtB->StartEngine_qgc_k == 10);
17884
17885 /* Switch: '<S21>/Switch8' incorporates:
17886 * Constant: '<S21>/Constant17'
17887 * Constant: '<S41>/Constant'
17888 * RelationalOperator: '<S41>/Compare'
17889 * SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3'
17890 */
17891 if (_rtB->LogicalOperator4) {
17892 _rtB->TakeoffUL = (int8_T)(_rtB->BusCreator2.TakeoffFg_qgc == 10);
17893 } else {
17894 _rtB->TakeoffUL = 0;
17895 }
17896
17897 /* End of Switch: '<S21>/Switch8' */
17898 }
17899
17900 if (ssIsSampleHit(S, 1, 0)) {
17901 /* RelationalOperator: '<S32>/Relational Operator8' incorporates:
17902 * Constant: '<S32>/Constant14'
17903 */
17904 _rtB->RelationalOperator8 = (_rtB->fmphase == 0);
17905
17906 /* RelationalOperator: '<S35>/Compare' incorporates:
17907 * Constant: '<S16>/Constant7'
17908 * Constant: '<S35>/Constant'
17909 */
17910 _rtB->RelationalOperator2 = false;
17911
17912 /* DataTypeConversion: '<S21>/Data Type Conversion2' */
17913 _rtB->DataTypeConversion2_m = (int8_T)_rtB->RelationalOperator2;
17914
17915 /* Chart: '<S21>/delayblock2' */
17916 /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock2 */
17917 /* During: AFCS_MODEL1/GNC/Calibration/delayblock2 */
17918 if (((uint8_T *)ssGetDWork(S, 606))[0] == 0U) {
17919 /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock2 */
17920 ((uint8_T *)ssGetDWork(S, 606))[0] = 1U;
17921
17922 /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock2 */
17923 ((uint8_T *)ssGetDWork(S, 607))[0] = AFCS_MODEL1_IN_delayblock;
17924
17925 /* Entry 'delayblock': '<S49>:38' */
17926 _rtB->Output_b = 0;
17927
17928 /* Entry Internal 'delayblock': '<S49>:38' */
17929 /* Transition: '<S49>:164' */
17930 if (_rtB->DataTypeConversion2_m == 0) {
17931 /* Transition: '<S49>:160' */
17932 ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Input0;
17933
17934 /* Entry 'Input0': '<S49>:138' */
17935 ((int32_T *)ssGetDWork(S, 341))[0] = 0;
17936 } else {
17937 /* Transition: '<S49>:166' */
17938 ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Input1;
17939
17940 /* Entry 'Input1': '<S49>:165' */
17941 ((int32_T *)ssGetDWork(S, 341))[0] = 0;
17942 }
17943 } else {
17944 /* During 'delayblock': '<S49>:38' */
17945 switch (((uint8_T *)ssGetDWork(S, 608))[0]) {
17946 case AFCS_MODEL1_IN_Input0:
17947 /* During 'Input0': '<S49>:138' */
17948 if (((int32_T *)ssGetDWork(S, 341))[0] == 20) {
17949 /* Transition: '<S49>:137' */
17950 ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Output0;
17951
17952 /* Entry 'Output0': '<S49>:130' */
17953 _rtB->Output_b = 0;
17954 ((int32_T *)ssGetDWork(S, 341))[0] = 0;
17955 } else if (_rtB->DataTypeConversion2_m == 1) {
17956 /* Transition: '<S49>:161' */
17957 ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Input1;
17958
17959 /* Entry 'Input1': '<S49>:165' */
17960 ((int32_T *)ssGetDWork(S, 341))[0] = 0;
17961 } else {
17962 ((int32_T *)ssGetDWork(S, 341))[0] = AFCS_MODEL1_addup_l(1, 20,
17963 ((int32_T *)ssGetDWork(S, 341))[0]);
17964 }
17965 break;
17966
17967 case AFCS_MODEL1_IN_Input1:
17968 /* During 'Input1': '<S49>:165' */
17969 if (((int32_T *)ssGetDWork(S, 341))[0] == 20) {
17970 /* Transition: '<S49>:168' */
17971 ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Output1;
17972
17973 /* Entry 'Output1': '<S49>:167' */
17974 _rtB->Output_b = 1;
17975 ((int32_T *)ssGetDWork(S, 341))[0] = 0;
17976 } else if (_rtB->DataTypeConversion2_m == 0) {
17977 /* Transition: '<S49>:162' */
17978 ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Input0;
17979
17980 /* Entry 'Input0': '<S49>:138' */
17981 ((int32_T *)ssGetDWork(S, 341))[0] = 0;
17982 } else {
17983 ((int32_T *)ssGetDWork(S, 341))[0] = AFCS_MODEL1_addup_l(1, 20,
17984 ((int32_T *)ssGetDWork(S, 341))[0]);
17985 }
17986 break;
17987
17988 case AFCS_MODEL1_IN_Output0:
17989 /* During 'Output0': '<S49>:130' */
17990 if (_rtB->DataTypeConversion2_m == 1) {
17991 /* Transition: '<S49>:139' */
17992 ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Input1;
17993
17994 /* Entry 'Input1': '<S49>:165' */
17995 ((int32_T *)ssGetDWork(S, 341))[0] = 0;
17996 }
17997 break;
17998
17999 default:
18000 /* During 'Output1': '<S49>:167' */
18001 if (_rtB->DataTypeConversion2_m == 0) {
18002 /* Transition: '<S49>:170' */
18003 ((uint8_T *)ssGetDWork(S, 608))[0] = AFCS_MODEL1_IN_Input0;
18004
18005 /* Entry 'Input0': '<S49>:138' */
18006 ((int32_T *)ssGetDWork(S, 341))[0] = 0;
18007 }
18008 break;
18009 }
18010 }
18011
18012 /* End of Chart: '<S21>/delayblock2' */
18013
18014 /* RelationalOperator: '<S32>/Relational Operator10' incorporates:
18015 * Constant: '<S32>/Constant18'
18016 */
18017 _rtB->RelationalOperator10 = (_rtB->Output_b == 1);
18018 }
18019
18020 if (1) {
18021 /* RelationalOperator: '<S32>/Relational Operator5' incorporates:
18022 * Constant: '<S32>/Constant12'
18023 */
18024 _rtB->LogicalOperator4 = (_rtB->LockFlag == 0);
18025
18026 /* Logic: '<S32>/Logical Operator3' */
18027 _rtB->LogicalOperator4 = (_rtB->RelationalOperator1 &&
18028 _rtB->LogicalOperator4);
18029
18030 /* Switch: '<S32>/Switch1' incorporates:
18031 * Constant: '<S32>/Constant4'
18032 */
18033 if (_rtB->LogicalOperator4) {
18034 _rtB->TakeoffFlag = _rtB->TakeoffUL;
18035 } else {
18036 _rtB->TakeoffFlag = 0;
18037 }
18038
18039 /* End of Switch: '<S32>/Switch1' */
18040
18041 /* RelationalOperator: '<S32>/Relational Operator9' incorporates:
18042 * Constant: '<S32>/Constant15'
18043 */
18044 _rtB->LogicalOperator4 = (_rtB->TakeoffFlag == 0);
18045
18046 /* RelationalOperator: '<S32>/Relational Operator7' incorporates:
18047 * Constant: '<S32>/Constant13'
18048 */
18049 _rtB->Compare_in = (_rtB->LockFlag == 0);
18050 }
18051
18052 if (ssIsSampleHit(S, 1, 0)) {
18053 /* UnitDelay: '<S32>/Unit Delay3' */
18054 _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 364))[0];
18055
18056 /* RelationalOperator: '<S32>/Relational Operator23' incorporates:
18057 * Constant: '<S32>/Constant36'
18058 */
18059 _rtB->RelationalOperator2 = (_rtB->UnitDelay5_b == 0);
18060
18061 /* RelationalOperator: '<S32>/Relational Operator18' incorporates:
18062 * Constant: '<S32>/Constant24'
18063 */
18064 _rtB->RelationalOperator12 = (1 == _rtB->fmphase);
18065
18066 /* Logic: '<S32>/Logical Operator16' */
18067 _rtB->RelationalOperator2 = (_rtB->RelationalOperator2 &&
18068 _rtB->RelationalOperator12);
18069
18070 /* RelationalOperator: '<S32>/Relational Operator12' incorporates:
18071 * Constant: '<S32>/Constant1'
18072 */
18073 _rtB->RelationalOperator12 = (_rtB->fmphase == 0);
18074
18075 /* RelationalOperator: '<S32>/Relational Operator19' incorporates:
18076 * Constant: '<S32>/Constant32'
18077 */
18078 _rtB->RelationalOperator19 = (_rtB->fmphase == 6);
18079
18080 /* RelationalOperator: '<S32>/Relational Operator20' incorporates:
18081 * Constant: '<S32>/Constant33'
18082 */
18083 _rtB->RelationalOperator20 = (_rtB->fmphase == 5);
18084 }
18085
18086 if (1) {
18087 /* Logic: '<S32>/Logical Operator4' */
18088 _rtB->LogicalOperator4 = (_rtB->LogicalOperator4 && _rtB->Compare_in &&
18089 _rtB->RelationalOperator8 && _rtB->RelationalOperator10);
18090
18091 /* Switch: '<S32>/Switch5' */
18092 _rtB->Swcheck = (int8_T)_rtB->LogicalOperator4;
18093
18094 /* RelationalOperator: '<S32>/Relational Operator11' incorporates:
18095 * Constant: '<S32>/Constant19'
18096 */
18097 _rtB->Compare_in = (_rtB->Swcheck == 1);
18098
18099 /* Logic: '<S32>/Logical Operator5' */
18100 _rtB->Compare_in = (_rtB->LogicalOperator_d || _rtB->Compare_in);
18101
18102 /* DataTypeConversion: '<S32>/Data Type Conversion' */
18103 _rtB->SWAUTO = (int8_T)_rtB->Compare_in;
18104
18105 /* RelationalOperator: '<S32>/Relational Operator21' incorporates:
18106 * Constant: '<S32>/Constant34'
18107 */
18108 _rtB->Compare_in = (_rtB->mh_j < 5.0F);
18109
18110 /* Logic: '<S32>/Logical Operator12' */
18111 _rtB->Compare_in = (_rtB->RelationalOperator20 && _rtB->Compare_in);
18112
18113 /* DataTypeConversion: '<S32>/Data Type Conversion7' */
18114 _rtB->DataTypeConversion7_k = (int8_T)_rtB->Compare_in;
18115
18116 /* RelationalOperator: '<S32>/Relational Operator22' incorporates:
18117 * Constant: '<S32>/Constant35'
18118 */
18119 _rtB->Compare_in = (_rtB->Nz > 1.3F);
18120
18121 /* DataTypeConversion: '<S32>/Data Type Conversion6' */
18122 _rtB->DataTypeConversion6_o = (int8_T)_rtB->Compare_in;
18123 }
18124
18125 if (ssIsSampleHit(S, 1, 0)) {
18126 /* Chart: '<S32>/wowcmp1' */
18127 /* Gateway: AFCS_MODEL1/GNC/setup/wowcmp1 */
18128 /* During: AFCS_MODEL1/GNC/setup/wowcmp1 */
18129 if (((uint8_T *)ssGetDWork(S, 524))[0] == 0U) {
18130 /* Entry: AFCS_MODEL1/GNC/setup/wowcmp1 */
18131 ((uint8_T *)ssGetDWork(S, 524))[0] = 1U;
18132
18133 /* Entry Internal: AFCS_MODEL1/GNC/setup/wowcmp1 */
18134 ((uint8_T *)ssGetDWork(S, 525))[0] = AFCS_MODEL1_IN_Enlowfg;
18135
18136 /* Entry 'Enlowfg': '<S331>:38' */
18137 _rtB->EnLowFlag_n = 0;
18138
18139 /* Entry Internal 'Enlowfg': '<S331>:38' */
18140 /* Transition: '<S331>:131' */
18141 ((uint8_T *)ssGetDWork(S, 526))[0] = AFCS_MODEL1_IN_count;
18142
18143 /* Entry 'count': '<S331>:138' */
18144 ((int32_T *)ssGetDWork(S, 296))[0] = 0;
18145 } else {
18146 /* During 'Enlowfg': '<S331>:38' */
18147 switch (((uint8_T *)ssGetDWork(S, 526))[0]) {
18148 case AFCS_MODEL1_IN_count:
18149 /* During 'count': '<S331>:138' */
18150 if (((int32_T *)ssGetDWork(S, 296))[0] == 200) {
18151 /* Transition: '<S331>:137' */
18152 ((uint8_T *)ssGetDWork(S, 526))[0] = AFCS_MODEL1_IN_enableFG;
18153
18154 /* Entry 'enableFG': '<S331>:130' */
18155 _rtB->EnLowFlag_n = 0;
18156 } else {
18157 ((int32_T *)ssGetDWork(S, 296))[0] = AFCS_MODEL1_addup_p
18158 (_rtB->DataTypeConversion7_k > 0, 200, ((int32_T *)ssGetDWork(S, 296))
18159 [0]);
18160 }
18161 break;
18162
18163 case AFCS_MODEL1_IN_enableFG:
18164 /* During 'enableFG': '<S331>:130' */
18165 if (_rtB->DataTypeConversion6_o > 0) {
18166 /* Transition: '<S331>:140' */
18167 ((uint8_T *)ssGetDWork(S, 526))[0] = AFCS_MODEL1_IN_enableFG1;
18168
18169 /* Entry 'enableFG1': '<S331>:139' */
18170 _rtB->EnLowFlag_n = 1;
18171 }
18172 break;
18173
18174 default:
18175 /* During 'enableFG1': '<S331>:139' */
18176 break;
18177 }
18178 }
18179
18180 /* End of Chart: '<S32>/wowcmp1' */
18181
18182 /* Logic: '<S32>/Logical Operator6' */
18183 _rtB->RelationalOperator_l = (_rtB->RelationalOperator2 ||
18184 _rtB->RelationalOperator12 || _rtB->RelationalOperator19 ||
18185 (_rtB->EnLowFlag_n != 0));
18186
18187 /* Switch: '<S32>/Switch6' */
18188 _rtB->Switch6_j = (int8_T)_rtB->RelationalOperator_l;
18189
18190 /* DataTypeConversion: '<S16>/Data Type Conversion' incorporates:
18191 * Constant: '<S16>/Constant'
18192 */
18193 _rtB->Divide_n = 0.0F;
18194
18195 /* Sum: '<S43>/Sum3' */
18196 _rtB->Divide_n -= AFCS_MODEL1_rtC(S)->Gain;
18197
18198 /* Product: '<S43>/Divide' */
18199 _rtB->Divide_n /= AFCS_MODEL1_rtC(S)->Sum_dn;
18200
18201 /* Saturate: '<S21>/Saturation' */
18202 if (_rtB->Divide_n > 1.0F) {
18203 _rtB->Saturation_l = 1.0F;
18204 } else if (_rtB->Divide_n < -1.0F) {
18205 _rtB->Saturation_l = -1.0F;
18206 } else {
18207 _rtB->Saturation_l = _rtB->Divide_n;
18208 }
18209
18210 /* End of Saturate: '<S21>/Saturation' */
18211
18212 /* DataTypeConversion: '<S16>/Data Type Conversion1' incorporates:
18213 * Constant: '<S16>/Constant1'
18214 */
18215 _rtB->Divide_n = 0.0F;
18216
18217 /* Sum: '<S44>/Sum3' */
18218 _rtB->Divide_n -= AFCS_MODEL1_rtC(S)->Gain_l;
18219
18220 /* Product: '<S44>/Divide' */
18221 _rtB->Divide_n /= AFCS_MODEL1_rtC(S)->Sum_es;
18222
18223 /* Saturate: '<S21>/Saturation1' */
18224 if (_rtB->Divide_n > 1.0F) {
18225 _rtB->Saturation1_k = 1.0F;
18226 } else if (_rtB->Divide_n < -1.0F) {
18227 _rtB->Saturation1_k = -1.0F;
18228 } else {
18229 _rtB->Saturation1_k = _rtB->Divide_n;
18230 }
18231
18232 /* End of Saturate: '<S21>/Saturation1' */
18233
18234 /* DataTypeConversion: '<S16>/Data Type Conversion3' incorporates:
18235 * Constant: '<S16>/Constant3'
18236 */
18237 _rtB->Divide_n = 0.0F;
18238
18239 /* Sum: '<S45>/Sum3' */
18240 _rtB->Divide_n -= AFCS_MODEL1_rtC(S)->Gain_m;
18241
18242 /* Product: '<S45>/Divide' */
18243 _rtB->Divide_n /= AFCS_MODEL1_rtC(S)->Sum_i;
18244
18245 /* Saturate: '<S21>/Saturation2' */
18246 if (_rtB->Divide_n > 1.0F) {
18247 _rtB->Saturation2 = 1.0F;
18248 } else if (_rtB->Divide_n < -1.0F) {
18249 _rtB->Saturation2 = -1.0F;
18250 } else {
18251 _rtB->Saturation2 = _rtB->Divide_n;
18252 }
18253
18254 /* End of Saturate: '<S21>/Saturation2' */
18255
18256 /* Sum: '<S47>/Sum' incorporates:
18257 * Constant: '<S21>/Constant3'
18258 * Constant: '<S21>/Constant6'
18259 */
18260 _rtB->Sum_h = 800.0F;
18261 }
18262
18263 if (1) {
18264 /* Switch: '<S21>/Switch' */
18265 _rtB->Switch_lo[0] = _rtB->Saturation_l;
18266 _rtB->Switch_lo[1] = _rtB->Saturation1_k;
18267 _rtB->Switch_lo[2] = _rtB->Saturation2;
18268
18269 /* Product: '<S47>/Divide' incorporates:
18270 * Constant: '<S21>/Constant6'
18271 * DataTypeConversion: '<S16>/Data Type Conversion2'
18272 * Sum: '<S16>/Sum3'
18273 * Sum: '<S47>/Sum3'
18274 * Switch: '<S21>/Switch'
18275 */
18276 DXp = -1100.0F / _rtB->Sum_h;
18277
18278 /* Saturate: '<S21>/Saturation3' incorporates:
18279 * Switch: '<S21>/Switch'
18280 */
18281 if (DXp > 1.0F) {
18282 _rtB->Switch_lo[3] = 1.0F;
18283 } else if (DXp < 0.0F) {
18284 _rtB->Switch_lo[3] = 0.0F;
18285 } else {
18286 _rtB->Switch_lo[3] = DXp;
18287 }
18288
18289 /* End of Saturate: '<S21>/Saturation3' */
18290
18291 /* Gain: '<S26>/Gain' */
18292 _rtB->Gain_e = 0.3F * _rtB->Switch_lo[3];
18293
18294 /* Saturate: '<S26>/Saturation' */
18295 if (_rtB->Gain_e > 0.3F) {
18296 _rtB->Saturation_n = 0.3F;
18297 } else if (_rtB->Gain_e < 0.0F) {
18298 _rtB->Saturation_n = 0.0F;
18299 } else {
18300 _rtB->Saturation_n = _rtB->Gain_e;
18301 }
18302
18303 /* End of Saturate: '<S26>/Saturation' */
18304
18305 /* RTW Generated Level2 S-Function Block: '<S306>/Generated S-Function15' (StickGradientDeadZone_sf) */
18306 {
18307 SimStruct *rts = ssGetSFunction(S, 167);
18308 sfcnOutputs(rts, 0);
18309 if (ssGetErrorStatus(rts) != (NULL))
18310 return;
18311 }
18312
18313 /* Gain: '<S26>/Gain1' */
18314 _rtB->Gain_e = -20.0F * _rtB->GeneratedSFunction15;
18315
18316 /* Saturate: '<S26>/Saturation1' */
18317 if (_rtB->Gain_e > 30.0F) {
18318 _rtB->Saturation1_f = 30.0F;
18319 } else if (_rtB->Gain_e < -30.0F) {
18320 _rtB->Saturation1_f = -30.0F;
18321 } else {
18322 _rtB->Saturation1_f = _rtB->Gain_e;
18323 }
18324
18325 /* End of Saturate: '<S26>/Saturation1' */
18326
18327 /* RTW Generated Level2 S-Function Block: '<S305>/Generated S-Function15' (StickGradientDeadZone_sf) */
18328 {
18329 SimStruct *rts = ssGetSFunction(S, 168);
18330 sfcnOutputs(rts, 0);
18331 if (ssGetErrorStatus(rts) != (NULL))
18332 return;
18333 }
18334
18335 /* Gain: '<S26>/Gain2' */
18336 _rtB->Gain_e = -20.0F * _rtB->GeneratedSFunction15_i;
18337
18338 /* Saturate: '<S26>/Saturation2' */
18339 if (_rtB->Gain_e > 30.0F) {
18340 _rtB->Saturation2_o = 30.0F;
18341 } else if (_rtB->Gain_e < -30.0F) {
18342 _rtB->Saturation2_o = -30.0F;
18343 } else {
18344 _rtB->Saturation2_o = _rtB->Gain_e;
18345 }
18346
18347 /* End of Saturate: '<S26>/Saturation2' */
18348
18349 /* RTW Generated Level2 S-Function Block: '<S304>/Generated S-Function15' (StickGradientDeadZone_sf) */
18350 {
18351 SimStruct *rts = ssGetSFunction(S, 169);
18352 sfcnOutputs(rts, 0);
18353 if (ssGetErrorStatus(rts) != (NULL))
18354 return;
18355 }
18356
18357 /* Gain: '<S26>/Gain3' */
18358 _rtB->Gain_e = -20.0F * _rtB->GeneratedSFunction15_e;
18359
18360 /* Saturate: '<S26>/Saturation3' */
18361 if (_rtB->Gain_e > 30.0F) {
18362 _rtB->Saturation3_f = 30.0F;
18363 } else if (_rtB->Gain_e < -30.0F) {
18364 _rtB->Saturation3_f = -30.0F;
18365 } else {
18366 _rtB->Saturation3_f = _rtB->Gain_e;
18367 }
18368
18369 /* End of Saturate: '<S26>/Saturation3' */
18370
18371 /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */
18372 _rtB->decl_qgc = _rtB->BusCreator2.decl_qgc;
18373
18374 /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */
18375 _rtB->dacl_qgc = _rtB->BusCreator2.dacl_qgc;
18376
18377 /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */
18378 _rtB->drcl_qgc = _rtB->BusCreator2.drcl_qgc;
18379
18380 /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */
18381 _rtB->nwscl_qgc = _rtB->BusCreator2.nwscl_qgc;
18382
18383 /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */
18384 _rtB->sbcl_qgc = _rtB->BusCreator2.sbcl_qgc;
18385
18386 /* DataTypeConversion: '<S1>/Data Type Conversion5' incorporates:
18387 * Inport: '<Root>/CMD_HdotC'
18388 */
18389 _rtB->CMD_Hdotc = *((const real32_T **)ssGetInputPortSignalPtrs(S, 4))[0];
18390
18391 /* DataTypeConversion: '<S24>/Data Type Conversion57' */
18392 _rtB->IndexAL = _rtB->IndexAL_k;
18393
18394 /* DataTypeConversion: '<S24>/Data Type Conversion3' */
18395 _rtB->DtoStart = _rtB->DtoStart_b;
18396 }
18397
18398 if (ssIsSampleHit(S, 1, 0)) {
18399 /* Switch: '<S32>/Switch7' */
18400 _rtB->Switch7_n = (int8_T)_rtB->RelationalOperator_l;
18401
18402 /* RelationalOperator: '<S38>/Compare' incorporates:
18403 * Constant: '<S16>/Constant5'
18404 * Constant: '<S38>/Constant'
18405 */
18406 _rtB->RelationalOperator_l = false;
18407
18408 /* DataTypeConversion: '<S21>/Data Type Conversion6' */
18409 _rtB->DataTypeConversion6_f = (int8_T)_rtB->RelationalOperator_l;
18410
18411 /* Chart: '<S21>/delayblock5' */
18412 /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock5 */
18413 /* During: AFCS_MODEL1/GNC/Calibration/delayblock5 */
18414 if (((uint8_T *)ssGetDWork(S, 597))[0] == 0U) {
18415 /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock5 */
18416 ((uint8_T *)ssGetDWork(S, 597))[0] = 1U;
18417
18418 /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock5 */
18419 ((uint8_T *)ssGetDWork(S, 598))[0] = AFCS_MODEL1_IN_delayblock;
18420
18421 /* Entry 'delayblock': '<S52>:38' */
18422 _rtB->Output_o = 0;
18423
18424 /* Entry Internal 'delayblock': '<S52>:38' */
18425 /* Transition: '<S52>:164' */
18426 if (_rtB->DataTypeConversion6_f == 0) {
18427 /* Transition: '<S52>:160' */
18428 ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Input0;
18429
18430 /* Entry 'Input0': '<S52>:138' */
18431 ((int32_T *)ssGetDWork(S, 338))[0] = 0;
18432 } else {
18433 /* Transition: '<S52>:166' */
18434 ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Input1;
18435
18436 /* Entry 'Input1': '<S52>:165' */
18437 ((int32_T *)ssGetDWork(S, 338))[0] = 0;
18438 }
18439 } else {
18440 /* During 'delayblock': '<S52>:38' */
18441 switch (((uint8_T *)ssGetDWork(S, 599))[0]) {
18442 case AFCS_MODEL1_IN_Input0:
18443 /* During 'Input0': '<S52>:138' */
18444 if (((int32_T *)ssGetDWork(S, 338))[0] == 20) {
18445 /* Transition: '<S52>:137' */
18446 ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Output0;
18447
18448 /* Entry 'Output0': '<S52>:130' */
18449 _rtB->Output_o = 0;
18450 ((int32_T *)ssGetDWork(S, 338))[0] = 0;
18451 } else if (_rtB->DataTypeConversion6_f == 1) {
18452 /* Transition: '<S52>:161' */
18453 ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Input1;
18454
18455 /* Entry 'Input1': '<S52>:165' */
18456 ((int32_T *)ssGetDWork(S, 338))[0] = 0;
18457 } else {
18458 ((int32_T *)ssGetDWork(S, 338))[0] = AFCS_MODEL1_addup_k(1, 20,
18459 ((int32_T *)ssGetDWork(S, 338))[0]);
18460 }
18461 break;
18462
18463 case AFCS_MODEL1_IN_Input1:
18464 /* During 'Input1': '<S52>:165' */
18465 if (((int32_T *)ssGetDWork(S, 338))[0] == 20) {
18466 /* Transition: '<S52>:168' */
18467 ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Output1;
18468
18469 /* Entry 'Output1': '<S52>:167' */
18470 _rtB->Output_o = 1;
18471 ((int32_T *)ssGetDWork(S, 338))[0] = 0;
18472 } else if (_rtB->DataTypeConversion6_f == 0) {
18473 /* Transition: '<S52>:162' */
18474 ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Input0;
18475
18476 /* Entry 'Input0': '<S52>:138' */
18477 ((int32_T *)ssGetDWork(S, 338))[0] = 0;
18478 } else {
18479 ((int32_T *)ssGetDWork(S, 338))[0] = AFCS_MODEL1_addup_k(1, 20,
18480 ((int32_T *)ssGetDWork(S, 338))[0]);
18481 }
18482 break;
18483
18484 case AFCS_MODEL1_IN_Output0:
18485 /* During 'Output0': '<S52>:130' */
18486 if (_rtB->DataTypeConversion6_f == 1) {
18487 /* Transition: '<S52>:139' */
18488 ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Input1;
18489
18490 /* Entry 'Input1': '<S52>:165' */
18491 ((int32_T *)ssGetDWork(S, 338))[0] = 0;
18492 }
18493 break;
18494
18495 default:
18496 /* During 'Output1': '<S52>:167' */
18497 if (_rtB->DataTypeConversion6_f == 0) {
18498 /* Transition: '<S52>:170' */
18499 ((uint8_T *)ssGetDWork(S, 599))[0] = AFCS_MODEL1_IN_Input0;
18500
18501 /* Entry 'Input0': '<S52>:138' */
18502 ((int32_T *)ssGetDWork(S, 338))[0] = 0;
18503 }
18504 break;
18505 }
18506 }
18507
18508 /* End of Chart: '<S21>/delayblock5' */
18509
18510 /* Logic: '<S32>/Logical Operator13' incorporates:
18511 * Constant: '<S13>/Constant9'
18512 */
18513 _rtB->RelationalOperator_l = true;
18514
18515 /* Logic: '<S32>/Logical Operator14' */
18516 _rtB->RelationalOperator_l = !_rtB->RelationalOperator_l;
18517
18518 /* DataTypeConversion: '<S32>/Data Type Conversion8' */
18519 _rtB->GPSFail = (int8_T)_rtB->RelationalOperator_l;
18520
18521 /* RelationalOperator: '<S32>/Relational Operator3' incorporates:
18522 * Constant: '<S32>/Constant7'
18523 */
18524 _rtB->RelationalOperator_l = (_rtB->fmphase == 1);
18525
18526 /* Switch: '<S32>/Switch3' incorporates:
18527 * Constant: '<S32>/Constant8'
18528 */
18529 if (_rtB->RelationalOperator_l) {
18530 /* Logic: '<S32>/Logical Operator15' */
18531 _rtB->LogicalOperator15 = ((_rtB->Output_o != 0) || (_rtB->GPSFail != 0));
18532
18533 /* DataTypeConversion: '<S32>/Data Type Conversion9' */
18534 _rtB->DataTypeConversion9_c = (int8_T)_rtB->LogicalOperator15;
18535 _rtB->Switch3 = _rtB->DataTypeConversion9_c;
18536 } else {
18537 _rtB->Switch3 = 0;
18538 }
18539
18540 /* End of Switch: '<S32>/Switch3' */
18541
18542 /* RelationalOperator: '<S32>/Relational Operator16' incorporates:
18543 * Constant: '<S32>/Constant11'
18544 */
18545 _rtB->RelationalOperator16 = (_rtB->fmphase == 5);
18546
18547 /* RelationalOperator: '<S39>/Compare' incorporates:
18548 * Constant: '<S16>/Constant4'
18549 * Constant: '<S39>/Constant'
18550 */
18551 _rtB->RelationalOperator_l = false;
18552
18553 /* DataTypeConversion: '<S21>/Data Type Conversion7' */
18554 _rtB->DataTypeConversion7_n = (int8_T)_rtB->RelationalOperator_l;
18555
18556 /* Chart: '<S21>/delayblock7' */
18557 /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock7 */
18558 /* During: AFCS_MODEL1/GNC/Calibration/delayblock7 */
18559 if (((uint8_T *)ssGetDWork(S, 591))[0] == 0U) {
18560 /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock7 */
18561 ((uint8_T *)ssGetDWork(S, 591))[0] = 1U;
18562
18563 /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock7 */
18564 ((uint8_T *)ssGetDWork(S, 592))[0] = AFCS_MODEL1_IN_delayblock;
18565
18566 /* Entry 'delayblock': '<S54>:38' */
18567 _rtB->Output_i = 0;
18568
18569 /* Entry Internal 'delayblock': '<S54>:38' */
18570 /* Transition: '<S54>:164' */
18571 if (_rtB->DataTypeConversion7_n == 0) {
18572 /* Transition: '<S54>:160' */
18573 ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Input0;
18574
18575 /* Entry 'Input0': '<S54>:138' */
18576 ((int32_T *)ssGetDWork(S, 336))[0] = 0;
18577 } else {
18578 /* Transition: '<S54>:166' */
18579 ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Input1;
18580
18581 /* Entry 'Input1': '<S54>:165' */
18582 ((int32_T *)ssGetDWork(S, 336))[0] = 0;
18583 }
18584 } else {
18585 /* During 'delayblock': '<S54>:38' */
18586 switch (((uint8_T *)ssGetDWork(S, 593))[0]) {
18587 case AFCS_MODEL1_IN_Input0:
18588 /* During 'Input0': '<S54>:138' */
18589 if (((int32_T *)ssGetDWork(S, 336))[0] == 20) {
18590 /* Transition: '<S54>:137' */
18591 ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Output0;
18592
18593 /* Entry 'Output0': '<S54>:130' */
18594 _rtB->Output_i = 0;
18595 ((int32_T *)ssGetDWork(S, 336))[0] = 0;
18596 } else if (_rtB->DataTypeConversion7_n == 1) {
18597 /* Transition: '<S54>:161' */
18598 ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Input1;
18599
18600 /* Entry 'Input1': '<S54>:165' */
18601 ((int32_T *)ssGetDWork(S, 336))[0] = 0;
18602 } else {
18603 ((int32_T *)ssGetDWork(S, 336))[0] = AFCS_MODEL1_addup_h(1, 20,
18604 ((int32_T *)ssGetDWork(S, 336))[0]);
18605 }
18606 break;
18607
18608 case AFCS_MODEL1_IN_Input1:
18609 /* During 'Input1': '<S54>:165' */
18610 if (((int32_T *)ssGetDWork(S, 336))[0] == 20) {
18611 /* Transition: '<S54>:168' */
18612 ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Output1;
18613
18614 /* Entry 'Output1': '<S54>:167' */
18615 _rtB->Output_i = 1;
18616 ((int32_T *)ssGetDWork(S, 336))[0] = 0;
18617 } else if (_rtB->DataTypeConversion7_n == 0) {
18618 /* Transition: '<S54>:162' */
18619 ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Input0;
18620
18621 /* Entry 'Input0': '<S54>:138' */
18622 ((int32_T *)ssGetDWork(S, 336))[0] = 0;
18623 } else {
18624 ((int32_T *)ssGetDWork(S, 336))[0] = AFCS_MODEL1_addup_h(1, 20,
18625 ((int32_T *)ssGetDWork(S, 336))[0]);
18626 }
18627 break;
18628
18629 case AFCS_MODEL1_IN_Output0:
18630 /* During 'Output0': '<S54>:130' */
18631 if (_rtB->DataTypeConversion7_n == 1) {
18632 /* Transition: '<S54>:139' */
18633 ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Input1;
18634
18635 /* Entry 'Input1': '<S54>:165' */
18636 ((int32_T *)ssGetDWork(S, 336))[0] = 0;
18637 }
18638 break;
18639
18640 default:
18641 /* During 'Output1': '<S54>:167' */
18642 if (_rtB->DataTypeConversion7_n == 0) {
18643 /* Transition: '<S54>:170' */
18644 ((uint8_T *)ssGetDWork(S, 593))[0] = AFCS_MODEL1_IN_Input0;
18645
18646 /* Entry 'Input0': '<S54>:138' */
18647 ((int32_T *)ssGetDWork(S, 336))[0] = 0;
18648 }
18649 break;
18650 }
18651 }
18652
18653 /* End of Chart: '<S21>/delayblock7' */
18654
18655 /* RelationalOperator: '<S32>/Relational Operator6' incorporates:
18656 * Constant: '<S32>/Constant28'
18657 */
18658 _rtB->RelationalOperator6 = (_rtB->Output_i == 1);
18659 }
18660
18661 if (1) {
18662 /* DataTypeConversion: '<S293>/Data Type Conversion3' */
18663 _rtB->PSDref = _rtB->Switch_oz[3];
18664
18665 /* DataTypeConversion: '<S293>/Data Type Conversion8' */
18666 _rtB->href = _rtB->Switch_oz[0];
18667
18668 /* DataTypeConversion: '<S293>/Data Type Conversion4' */
18669 _rtB->drange = _rtB->Switch_oz[4];
18670
18671 /* DataTypeConversion: '<S293>/Data Type Conversion2' */
18672 _rtB->xrw = _rtB->Switch_oz[1];
18673
18674 /* RelationalOperator: '<S32>/Relational Operator15' incorporates:
18675 * Constant: '<S32>/Constant10'
18676 */
18677 _rtB->Compare_in = (_rtB->LockFlag == 0);
18678 }
18679
18680 if (ssIsSampleHit(S, 1, 0)) {
18681 /* Constant: '<S13>/Constant23' */
18682 _rtB->EnLowFlag = 0;
18683
18684 /* Chart: '<S21>/delayblock4' */
18685 /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock4 */
18686 /* During: AFCS_MODEL1/GNC/Calibration/delayblock4 */
18687 if (((uint8_T *)ssGetDWork(S, 600))[0] == 0U) {
18688 /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock4 */
18689 ((uint8_T *)ssGetDWork(S, 600))[0] = 1U;
18690
18691 /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock4 */
18692 ((uint8_T *)ssGetDWork(S, 601))[0] = AFCS_MODEL1_IN_delayblock;
18693
18694 /* Entry 'delayblock': '<S51>:38' */
18695 _rtB->Output_c = 0;
18696
18697 /* Entry Internal 'delayblock': '<S51>:38' */
18698 /* Transition: '<S51>:164' */
18699 if (AFCS_MODEL1_rtC(S)->DataTypeConversion5 == 0) {
18700 /* Transition: '<S51>:160' */
18701 ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Input0;
18702
18703 /* Entry 'Input0': '<S51>:138' */
18704 ((int32_T *)ssGetDWork(S, 339))[0] = 0;
18705 } else {
18706 /* Transition: '<S51>:166' */
18707 ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Input1;
18708
18709 /* Entry 'Input1': '<S51>:165' */
18710 ((int32_T *)ssGetDWork(S, 339))[0] = 0;
18711 }
18712 } else {
18713 /* During 'delayblock': '<S51>:38' */
18714 switch (((uint8_T *)ssGetDWork(S, 602))[0]) {
18715 case AFCS_MODEL1_IN_Input0:
18716 /* During 'Input0': '<S51>:138' */
18717 if (((int32_T *)ssGetDWork(S, 339))[0] == 20) {
18718 /* Transition: '<S51>:137' */
18719 ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Output0;
18720
18721 /* Entry 'Output0': '<S51>:130' */
18722 _rtB->Output_c = 0;
18723 ((int32_T *)ssGetDWork(S, 339))[0] = 0;
18724 } else if (AFCS_MODEL1_rtC(S)->DataTypeConversion5 == 1) {
18725 /* Transition: '<S51>:161' */
18726 ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Input1;
18727
18728 /* Entry 'Input1': '<S51>:165' */
18729 ((int32_T *)ssGetDWork(S, 339))[0] = 0;
18730 } else {
18731 ((int32_T *)ssGetDWork(S, 339))[0] = AFCS_MODEL1_addup_i(1, 20,
18732 ((int32_T *)ssGetDWork(S, 339))[0]);
18733 }
18734 break;
18735
18736 case AFCS_MODEL1_IN_Input1:
18737 /* During 'Input1': '<S51>:165' */
18738 if (((int32_T *)ssGetDWork(S, 339))[0] == 20) {
18739 /* Transition: '<S51>:168' */
18740 ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Output1;
18741
18742 /* Entry 'Output1': '<S51>:167' */
18743 _rtB->Output_c = 1;
18744 ((int32_T *)ssGetDWork(S, 339))[0] = 0;
18745 } else if (AFCS_MODEL1_rtC(S)->DataTypeConversion5 == 0) {
18746 /* Transition: '<S51>:162' */
18747 ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Input0;
18748
18749 /* Entry 'Input0': '<S51>:138' */
18750 ((int32_T *)ssGetDWork(S, 339))[0] = 0;
18751 } else {
18752 ((int32_T *)ssGetDWork(S, 339))[0] = AFCS_MODEL1_addup_i(1, 20,
18753 ((int32_T *)ssGetDWork(S, 339))[0]);
18754 }
18755 break;
18756
18757 case AFCS_MODEL1_IN_Output0:
18758 /* During 'Output0': '<S51>:130' */
18759 if (AFCS_MODEL1_rtC(S)->DataTypeConversion5 == 1) {
18760 /* Transition: '<S51>:139' */
18761 ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Input1;
18762
18763 /* Entry 'Input1': '<S51>:165' */
18764 ((int32_T *)ssGetDWork(S, 339))[0] = 0;
18765 }
18766 break;
18767
18768 default:
18769 /* During 'Output1': '<S51>:167' */
18770 if (AFCS_MODEL1_rtC(S)->DataTypeConversion5 == 0) {
18771 /* Transition: '<S51>:170' */
18772 ((uint8_T *)ssGetDWork(S, 602))[0] = AFCS_MODEL1_IN_Input0;
18773
18774 /* Entry 'Input0': '<S51>:138' */
18775 ((int32_T *)ssGetDWork(S, 339))[0] = 0;
18776 }
18777 break;
18778 }
18779 }
18780
18781 /* End of Chart: '<S21>/delayblock4' */
18782
18783 /* RelationalOperator: '<S32>/Relational Operator4' incorporates:
18784 * Constant: '<S32>/Constant5'
18785 */
18786 _rtB->RelationalOperator_l = (_rtB->fmphase == 3);
18787
18788 /* RelationalOperator: '<S32>/Relational Operator2' incorporates:
18789 * Constant: '<S32>/Constant9'
18790 */
18791 _rtB->RelationalOperator2 = (_rtB->fmphase == 4);
18792
18793 /* Logic: '<S32>/Logical Operator9' */
18794 _rtB->RelationalOperator_l = (_rtB->RelationalOperator_l ||
18795 _rtB->RelationalOperator2);
18796
18797 /* Switch: '<S32>/Switch2' incorporates:
18798 * Constant: '<S32>/Constant6'
18799 */
18800 if (_rtB->RelationalOperator_l) {
18801 _rtB->EmlandFg = _rtB->Output_c;
18802 } else {
18803 _rtB->EmlandFg = 0;
18804 }
18805
18806 /* End of Switch: '<S32>/Switch2' */
18807 }
18808
18809 if (1) {
18810 /* Logic: '<S32>/Logical Operator10' */
18811 _rtB->Compare_in = (_rtB->Compare_in && _rtB->RelationalOperator16 &&
18812 _rtB->RelationalOperator6);
18813
18814 /* Switch: '<S32>/Switch4' */
18815 _rtB->GoaroundFg = (int8_T)_rtB->Compare_in;
18816
18817 /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */
18818 _rtB->Gain_e = _rtB->BusCreator2.guidance.altcmd;
18819
18820 /* Saturate: '<S31>/Saturation2' */
18821 if (_rtB->Gain_e > 5000.0F) {
18822 _rtB->Saturation2_g = 5000.0F;
18823 } else if (_rtB->Gain_e < 10.0F) {
18824 _rtB->Saturation2_g = 10.0F;
18825 } else {
18826 _rtB->Saturation2_g = _rtB->Gain_e;
18827 }
18828
18829 /* End of Saturate: '<S31>/Saturation2' */
18830
18831 /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */
18832 _rtB->ascmd = _rtB->BusCreator2.guidance.ascmd;
18833
18834 /* MATLAB Function: '<S31>/MATLAB Function1' */
18835 /* MATLAB Function 'AFCS_MODEL1/GNC/logic/MATLAB Function1': '<S318>:1' */
18836 /* '<S318>:1:3' */
18837 _rtB->y = (real32_T)atan(_rtB->ascmd / 9.8F) * 1.2F * 57.3F;
18838
18839 /* Saturate: '<S31>/Saturation' */
18840 if (_rtB->y > 30.0F) {
18841 _rtB->Saturation_h = 30.0F;
18842 } else if (_rtB->y < -30.0F) {
18843 _rtB->Saturation_h = -30.0F;
18844 } else {
18845 _rtB->Saturation_h = _rtB->y;
18846 }
18847
18848 /* End of Saturate: '<S31>/Saturation' */
18849
18850 /* SignalConversion: '<S1>/BusConversion_InsertedFor_Bus Creator_at_inport_3' */
18851 _rtB->targetpoint = _rtB->BusCreator2.guidance.targetpoint;
18852
18853 /* MATLAB Function: '<S320>/MATLAB Function2' */
18854 /* MATLAB Function 'AFCS_MODEL1/GNC/logic/Subsystem2/MATLAB Function2': '<S328>:1' */
18855 if (_rtB->targetpoint < 2) {
18856 /* '<S328>:1:3' */
18857 /* '<S328>:1:4' */
18858 _rtB->y_p = 2;
18859
18860 /* 不能小于2,前两个点的高度是0 */
18861 } else {
18862 i_1 = _rtB->NumPoint - 3;
18863 if (i_1 < -128) {
18864 i_1 = -128;
18865 }
18866
18867 if (_rtB->targetpoint > i_1) {
18868 /* '<S328>:1:5' */
18869 /* '<S328>:1:6' */
18870 i_1 = _rtB->NumPoint - 3;
18871 if (i_1 < -128) {
18872 i_1 = -128;
18873 }
18874
18875 _rtB->y_p = (int8_T)i_1;
18876
18877 /* 程序里k从1开始,最后两个点的高度是0,倒数第三个点有进场标志,所以不能超过倒数第四个点。 */
18878 } else {
18879 /* '<S328>:1:8' */
18880 _rtB->y_p = _rtB->targetpoint;
18881 }
18882 }
18883
18884 /* End of MATLAB Function: '<S320>/MATLAB Function2' */
18885
18886 /* SwitchCase: '<S31>/Switch Case' */
18887 rtAction = -1;
18888 if (ssIsMajorTimeStep(S)) {
18889 switch (_rtB->fmphase) {
18890 case 0:
18891 rtAction = 0;
18892 break;
18893
18894 case 1:
18895 rtAction = 1;
18896 break;
18897
18898 case 2:
18899 rtAction = 2;
18900 break;
18901
18902 case 3:
18903 rtAction = 3;
18904 break;
18905
18906 case 4:
18907 rtAction = 4;
18908 break;
18909
18910 case 5:
18911 rtAction = 5;
18912 break;
18913
18914 case 6:
18915 rtAction = 6;
18916 break;
18917 }
18918
18919 ((int8_T *)ssGetDWork(S, 505))[0] = rtAction;
18920 } else {
18921 rtAction = ((int8_T *)ssGetDWork(S, 505))[0];
18922 }
18923
18924 switch (rtAction) {
18925 case 0:
18926 /* Outputs for IfAction SubSystem: '<S31>/Pre-Takeoff' incorporates:
18927 * ActionPort: '<S319>/Action Port'
18928 */
18929 if (ssIsSampleHit(S, 1, 0)) {
18930 /* Chart: '<S319>/Pre_Takeoff' */
18931 /* Gateway: AFCS_MODEL1/GNC/logic/Pre-Takeoff/Pre_Takeoff */
18932 /* During: AFCS_MODEL1/GNC/logic/Pre-Takeoff/Pre_Takeoff */
18933 if (((uint8_T *)ssGetDWork(S, 580))[0] == 0U) {
18934 /* Entry: AFCS_MODEL1/GNC/logic/Pre-Takeoff/Pre_Takeoff */
18935 ((uint8_T *)ssGetDWork(S, 580))[0] = 1U;
18936
18937 /* Entry Internal: AFCS_MODEL1/GNC/logic/Pre-Takeoff/Pre_Takeoff */
18938 ((uint8_T *)ssGetDWork(S, 581))[0] = AFCS_MODEL1_IN_Pre_Takeoff;
18939
18940 /* Entry 'Pre_Takeoff': '<S327>:38' */
18941 _rtB->configure_cu = 1;
18942 _rtB->apthL_p = 1;
18943 _rtB->aplonL_fl = 1;
18944 _rtB->aplatL_a1 = 1;
18945 _rtB->apyawL_e = 1;
18946 _rtB->apsbL_i = 1;
18947 _rtB->apnwsL_e = 1;
18948 _rtB->apbrkL_p = 2;
18949 _rtB->thrustcmd_c = 0.0F;
18950 _rtB->vtcmd_ld = 0.0F;
18951 _rtB->nzcmd_c = 0.0F;
18952 _rtB->nycmd_d = 0.0F;
18953 _rtB->phiLcmd_e = 0.0F;
18954 _rtB->thetaLcmd_n = 0.0F;
18955 _rtB->psiLcmd_a = 0.0F;
18956 _rtB->gamacmd_a = 0.0F;
18957 _rtB->hcmd_jv = 0.0F;
18958 _rtB->psdcmd_j = 0.0F;
18959 _rtB->yrwcmd_g = 0.0F;
18960 _rtB->fmpretotakeoff = 0;
18961 _rtB->StartEmNav_m = 0;
18962
18963 /* Entry Internal 'Pre_Takeoff': '<S327>:38' */
18964 /* Transition: '<S327>:36' */
18965 ((uint8_T *)ssGetDWork(S, 582))[0] = AFCS_MODEL1_IN_LockState;
18966
18967 /* Entry 'LockState': '<S327>:28' */
18968 ((int32_T *)ssGetDWork(S, 330))[0] = 0;
18969 _rtB->apcheck_m = 0;
18970 _rtB->apfix_o = 1;
18971 } else {
18972 /* During 'Pre_Takeoff': '<S327>:38' */
18973 _rtB->K_i = 1;
18974 switch (((uint8_T *)ssGetDWork(S, 582))[0]) {
18975 case AFCS_MODEL1_IN_LockState:
18976 /* During 'LockState': '<S327>:28' */
18977 if (((int32_T *)ssGetDWork(S, 330))[0] == 5) {
18978 /* Transition: '<S327>:30' */
18979 ((uint8_T *)ssGetDWork(S, 582))[0] = AFCS_MODEL1_IN_StickCheck;
18980
18981 /* Entry 'StickCheck': '<S327>:29' */
18982 _rtB->apfix_o = 0;
18983 _rtB->apcheck_m = 1;
18984 ((int32_T *)ssGetDWork(S, 329))[0] = 0;
18985 ((int32_T *)ssGetDWork(S, 328))[0] = 0;
18986 } else {
18987 i_1 = 1 - _rtB->LockFlag;
18988 if (i_1 > 127) {
18989 i_1 = 127;
18990 }
18991
18992 ((int32_T *)ssGetDWork(S, 330))[0] = AFCS_MODEL1_addup_ke((int8_T)
18993 i_1, 5, ((int32_T *)ssGetDWork(S, 330))[0]);
18994 }
18995 break;
18996
18997 case AFCS_MODEL1_IN_StickCheck:
18998 /* During 'StickCheck': '<S327>:29' */
18999 if (((int32_T *)ssGetDWork(S, 329))[0] == 5) {
19000 /* Transition: '<S327>:43' */
19001 ((uint8_T *)ssGetDWork(S, 582))[0] = AFCS_MODEL1_IN_TransTakeoff;
19002
19003 /* Entry 'TransTakeoff': '<S327>:42' */
19004 _rtB->apfix_o = 0;
19005 _rtB->apcheck_m = 0;
19006
19007 /* Entry Internal 'TransTakeoff': '<S327>:42' */
19008 /* Transition: '<S327>:92' */
19009 if (_rtB->GPSFail == 0) {
19010 /* Transition: '<S327>:90' */
19011 _rtB->fmpretotakeoff = 1;
19012 } else {
19013 /* Transition: '<S327>:91' */
19014 _rtB->fmpretotakeoff = 0;
19015 }
19016 } else if (((int32_T *)ssGetDWork(S, 328))[0] == 5) {
19017 /* Transition: '<S327>:79' */
19018 ((uint8_T *)ssGetDWork(S, 582))[0] = AFCS_MODEL1_IN_LockState;
19019
19020 /* Entry 'LockState': '<S327>:28' */
19021 ((int32_T *)ssGetDWork(S, 330))[0] = 0;
19022 _rtB->apcheck_m = 0;
19023 _rtB->apfix_o = 1;
19024 } else {
19025 ((int32_T *)ssGetDWork(S, 329))[0] = AFCS_MODEL1_addup_ke
19026 (_rtB->TakeoffFlag, 5, ((int32_T *)ssGetDWork(S, 329))[0]);
19027 ((int32_T *)ssGetDWork(S, 328))[0] = AFCS_MODEL1_addup_ke
19028 (_rtB->LockFlag, 5, ((int32_T *)ssGetDWork(S, 328))[0]);
19029 }
19030 break;
19031
19032 default:
19033 /* During 'TransTakeoff': '<S327>:42' */
19034 break;
19035 }
19036 }
19037
19038 /* End of Chart: '<S319>/Pre_Takeoff' */
19039 }
19040
19041 if (1) {
19042 /* SignalConversion: '<S319>/Signal Conversion' */
19043 _rtB->configure = _rtB->configure_cu;
19044
19045 /* SignalConversion: '<S319>/Signal Conversion' */
19046 _rtB->vtcmd = _rtB->vtcmd_ld;
19047
19048 /* SignalConversion: '<S319>/Signal Conversion' */
19049 _rtB->nzcmd = _rtB->nzcmd_c;
19050
19051 /* SignalConversion: '<S319>/Signal Conversion' */
19052 _rtB->nycmd = _rtB->nycmd_d;
19053
19054 /* SignalConversion: '<S319>/Signal Conversion' */
19055 _rtB->phiLcmd = _rtB->phiLcmd_e;
19056
19057 /* SignalConversion: '<S319>/Signal Conversion' */
19058 _rtB->thetaLcmd = _rtB->thetaLcmd_n;
19059
19060 /* SignalConversion: '<S319>/Signal Conversion' */
19061 _rtB->psiLcmd = _rtB->psiLcmd_a;
19062
19063 /* SignalConversion: '<S319>/Signal Conversion' */
19064 _rtB->gamacmd = _rtB->gamacmd_a;
19065
19066 /* SignalConversion: '<S319>/Signal Conversion' */
19067 _rtB->hcmd = _rtB->hcmd_jv;
19068
19069 /* SignalConversion: '<S319>/Signal Conversion' */
19070 _rtB->psdcmd = _rtB->psdcmd_j;
19071
19072 /* SignalConversion: '<S319>/Signal Conversion' */
19073 _rtB->yrwcmd = _rtB->yrwcmd_g;
19074
19075 /* SignalConversion: '<S319>/Signal Conversion' */
19076 _rtB->apthL = _rtB->apthL_p;
19077
19078 /* SignalConversion: '<S319>/Signal Conversion' */
19079 _rtB->apcheck = _rtB->apcheck_m;
19080
19081 /* SignalConversion: '<S319>/Signal Conversion' */
19082 _rtB->apfix = _rtB->apfix_o;
19083
19084 /* SignalConversion: '<S319>/Signal Conversion' */
19085 _rtB->K = _rtB->K_i;
19086
19087 /* SignalConversion: '<S319>/Signal Conversion' */
19088 _rtB->StartEmNav = _rtB->StartEmNav_m;
19089
19090 /* SignalConversion: '<S319>/Signal Conversion' */
19091 _rtB->aplonL = _rtB->aplonL_fl;
19092
19093 /* SignalConversion: '<S319>/Signal Conversion' */
19094 _rtB->aplatL = _rtB->aplatL_a1;
19095
19096 /* SignalConversion: '<S319>/Signal Conversion' */
19097 _rtB->apyawL = _rtB->apyawL_e;
19098
19099 /* SignalConversion: '<S319>/Signal Conversion' */
19100 _rtB->apsbL = _rtB->apsbL_i;
19101
19102 /* SignalConversion: '<S319>/Signal Conversion' */
19103 _rtB->apnwsL = _rtB->apnwsL_e;
19104
19105 /* SignalConversion: '<S319>/Signal Conversion' */
19106 _rtB->apbrkL = _rtB->apbrkL_p;
19107
19108 /* SignalConversion: '<S319>/Signal Conversion' */
19109 _rtB->thrustcmd = _rtB->thrustcmd_c;
19110 }
19111
19112 /* End of Outputs for SubSystem: '<S31>/Pre-Takeoff' */
19113 break;
19114
19115 case 1:
19116 /* Outputs for IfAction SubSystem: '<S31>/Takeoff' incorporates:
19117 * ActionPort: '<S321>/Action Port'
19118 */
19119 if (ssIsSampleHit(S, 1, 0)) {
19120 /* Chart: '<S321>/Takeoff' incorporates:
19121 * Constant: '<S17>/Constant9'
19122 */
19123 /* Gateway: AFCS_MODEL1/GNC/logic/Takeoff/Takeoff */
19124 /* During: AFCS_MODEL1/GNC/logic/Takeoff/Takeoff */
19125 if (((uint8_T *)ssGetDWork(S, 576))[0] == 0U) {
19126 /* Entry: AFCS_MODEL1/GNC/logic/Takeoff/Takeoff */
19127 ((uint8_T *)ssGetDWork(S, 576))[0] = 1U;
19128
19129 /* Entry Internal: AFCS_MODEL1/GNC/logic/Takeoff/Takeoff */
19130 ((uint8_T *)ssGetDWork(S, 577))[0] = AFCS_MODEL1_IN_Takeoff;
19131
19132 /* Entry 'Takeoff': '<S329>:38' */
19133 _rtB->apsbL_l1 = 1;
19134 _rtB->vtcmd_e = 0.0F;
19135 _rtB->nzcmd_iw = 0.0F;
19136 _rtB->nycmd_b = 0.0F;
19137 _rtB->gamacmd_d = 0.0F;
19138 _rtB->hcmd_kk = 0.0F;
19139 _rtB->psdcmd_b = 0.0F;
19140 _rtB->yrwcmd_m = 0.0F;
19141 _rtB->apfix_hs = 0;
19142 _rtB->apcheck_i = 0;
19143 _rtB->fmTOtoClimb = 0;
19144 _rtB->fmTOtoTaxi = 0;
19145 _rtB->K_cw = 1;
19146 _rtB->StartEmNav_f = 0;
19147 _rtB->LiftWheelFg = 0;
19148 _rtB->AbortTakeoffFg = 0;
19149
19150 /* Entry Internal 'Takeoff': '<S329>:38' */
19151 /* Transition: '<S329>:36' */
19152 ((uint8_T *)ssGetDWork(S, 578))[0] = AFCS_MODEL1_IN_NormalTakeoff;
19153
19154 /* Entry 'NormalTakeoff': '<S329>:136' */
19155 _rtB->configure_f = 1;
19156 _rtB->apthL_i2 = 1;
19157 _rtB->aplonL_ik = 1;
19158 _rtB->aplatL_d = 1;
19159 _rtB->apyawL_g = 1;
19160 _rtB->apnwsL_g = 1;
19161
19162 /* 由2改为1 */
19163 _rtB->apbrkL_h = 1;
19164 _rtB->thrustcmd_g = 0.85F;
19165 _rtB->phiLcmd_h = 0.0F;
19166 _rtB->thetaLcmd_ae = 0.0F;
19167 _rtB->psiLcmd_p = 0.0F;
19168 ((int32_T *)ssGetDWork(S, 326))[0] = 0;
19169
19170 /* Entry Internal 'NormalTakeoff': '<S329>:136' */
19171 /* Transition: '<S329>:195' */
19172 ((uint8_T *)ssGetDWork(S, 579))[0] = AFCS_MODEL1_IN_Initial;
19173
19174 /* Entry 'Initial': '<S329>:193' */
19175 ((int32_T *)ssGetDWork(S, 327))[0] = 0;
19176 } else {
19177 /* During 'Takeoff': '<S329>:38' */
19178 if (((uint8_T *)ssGetDWork(S, 578))[0] != AFCS_MODEL1_IN_AbortTakeoff)
19179 {
19180 AFCS_MODEL1_NormalTakeoff(S);
19181 } else {
19182 /* During 'AbortTakeoff': '<S329>:152' */
19183 }
19184 }
19185
19186 /* End of Chart: '<S321>/Takeoff' */
19187 }
19188
19189 if (1) {
19190 /* SignalConversion: '<S321>/Signal Conversion' */
19191 _rtB->configure = _rtB->configure_f;
19192
19193 /* SignalConversion: '<S321>/Signal Conversion' */
19194 _rtB->vtcmd = _rtB->vtcmd_e;
19195
19196 /* SignalConversion: '<S321>/Signal Conversion' */
19197 _rtB->nzcmd = _rtB->nzcmd_iw;
19198
19199 /* SignalConversion: '<S321>/Signal Conversion' */
19200 _rtB->nycmd = _rtB->nycmd_b;
19201
19202 /* SignalConversion: '<S321>/Signal Conversion' */
19203 _rtB->phiLcmd = _rtB->phiLcmd_h;
19204
19205 /* SignalConversion: '<S321>/Signal Conversion' */
19206 _rtB->thetaLcmd = _rtB->thetaLcmd_ae;
19207
19208 /* SignalConversion: '<S321>/Signal Conversion' */
19209 _rtB->psiLcmd = _rtB->psiLcmd_p;
19210
19211 /* SignalConversion: '<S321>/Signal Conversion' */
19212 _rtB->gamacmd = _rtB->gamacmd_d;
19213
19214 /* SignalConversion: '<S321>/Signal Conversion' */
19215 _rtB->hcmd = _rtB->hcmd_kk;
19216
19217 /* SignalConversion: '<S321>/Signal Conversion' */
19218 _rtB->psdcmd = _rtB->psdcmd_b;
19219
19220 /* SignalConversion: '<S321>/Signal Conversion' */
19221 _rtB->yrwcmd = _rtB->yrwcmd_m;
19222
19223 /* SignalConversion: '<S321>/Signal Conversion' */
19224 _rtB->apthL = _rtB->apthL_i2;
19225
19226 /* SignalConversion: '<S321>/Signal Conversion' */
19227 _rtB->apcheck = _rtB->apcheck_i;
19228
19229 /* SignalConversion: '<S321>/Signal Conversion' */
19230 _rtB->apfix = _rtB->apfix_hs;
19231
19232 /* SignalConversion: '<S321>/Signal Conversion' */
19233 _rtB->K = _rtB->K_cw;
19234
19235 /* SignalConversion: '<S321>/Signal Conversion' */
19236 _rtB->StartEmNav = _rtB->StartEmNav_f;
19237
19238 /* SignalConversion: '<S321>/Signal Conversion' */
19239 _rtB->aplonL = _rtB->aplonL_ik;
19240
19241 /* SignalConversion: '<S321>/Signal Conversion' */
19242 _rtB->aplatL = _rtB->aplatL_d;
19243
19244 /* SignalConversion: '<S321>/Signal Conversion' */
19245 _rtB->apyawL = _rtB->apyawL_g;
19246
19247 /* SignalConversion: '<S321>/Signal Conversion' */
19248 _rtB->apsbL = _rtB->apsbL_l1;
19249
19250 /* SignalConversion: '<S321>/Signal Conversion' */
19251 _rtB->apnwsL = _rtB->apnwsL_g;
19252
19253 /* SignalConversion: '<S321>/Signal Conversion' */
19254 _rtB->apbrkL = _rtB->apbrkL_h;
19255
19256 /* SignalConversion: '<S321>/Signal Conversion' */
19257 _rtB->thrustcmd = _rtB->thrustcmd_g;
19258 }
19259
19260 /* End of Outputs for SubSystem: '<S31>/Takeoff' */
19261 break;
19262
19263 case 2:
19264 /* Outputs for IfAction SubSystem: '<S31>/Climb' incorporates:
19265 * ActionPort: '<S314>/Action Port'
19266 */
19267 if (ssIsSampleHit(S, 1, 0)) {
19268 /* Chart: '<S314>/Climb2' incorporates:
19269 * Constant: '<S17>/Constant10'
19270 * Constant: '<S17>/Constant15'
19271 * Constant: '<S17>/Constant16'
19272 * Constant: '<S17>/Constant6'
19273 * Constant: '<S17>/Constant9'
19274 */
19275 /* Gateway: AFCS_MODEL1/GNC/logic/Climb/Climb2 */
19276 /* During: AFCS_MODEL1/GNC/logic/Climb/Climb2 */
19277 if (((uint8_T *)ssGetDWork(S, 567))[0] == 0U) {
19278 /* Entry: AFCS_MODEL1/GNC/logic/Climb/Climb2 */
19279 ((uint8_T *)ssGetDWork(S, 567))[0] = 1U;
19280
19281 /* Entry Internal: AFCS_MODEL1/GNC/logic/Climb/Climb2 */
19282 ((uint8_T *)ssGetDWork(S, 568))[0] = AFCS_MODEL1_IN_Climb;
19283
19284 /* Entry 'Climb': '<S324>:38' */
19285 _rtB->configure_o = 1;
19286 _rtB->apsbL_ex = 1;
19287 _rtB->apnwsL_ku = 1;
19288 _rtB->apbrkL_b = 1;
19289 _rtB->thrustcmd_k = 0.85F;
19290 _rtB->vtcmd_h1 = 0.0F;
19291 _rtB->nzcmd_o = 0.0F;
19292 _rtB->nycmd_n = 0.0F;
19293 _rtB->psiLcmd_j = 0.0F;
19294 _rtB->gamacmd_j = 0.0F;
19295 _rtB->hcmd_eq = _rtB->href;
19296 _rtB->apfix_h5 = 0;
19297 _rtB->apcheck_c = 0;
19298 _rtB->fmClimbtoCruise = 0;
19299 _rtB->K_j = 2;
19300 _rtB->StartEmNav_a2 = 0;
19301
19302 /* Entry Internal 'Climb': '<S324>:38' */
19303 /* Transition: '<S324>:95' */
19304 ((uint8_T *)ssGetDWork(S, 569))[0] = AFCS_MODEL1_IN_PositionControl;
19305
19306 /* Entry 'PositionControl': '<S324>:29' */
19307 _rtB->apthL_fb = 1;
19308 _rtB->aplonL_b = 2;
19309 _rtB->phiLcmd_o = 0.0F;
19310 _rtB->thetaLcmd_g = 5.0F;
19311 _rtB->yrwcmd_a = 0.0F;
19312 ((int32_T *)ssGetDWork(S, 320))[0] = 0;
19313
19314 /* Entry Internal 'PositionControl': '<S324>:29' */
19315 ((uint8_T *)ssGetDWork(S, 570))[0] = 1U;
19316 ((uint8_T *)ssGetDWork(S, 572))[0] = 1U;
19317
19318 /* Entry Internal 'XYPositionControl': '<S324>:131' */
19319 /* Transition: '<S324>:133' */
19320 ((uint8_T *)ssGetDWork(S, 571))[0] = AFCS_MODEL1_IN_PositionControl1;
19321
19322 /* Entry 'PositionControl1': '<S324>:135' */
19323 /* Entry Internal 'PositionControl1': '<S324>:135' */
19324 /* Transition: '<S324>:215' */
19325 ((uint8_T *)ssGetDWork(S, 573))[0] = AFCS_MODEL1_IN_YrwControl;
19326
19327 /* Entry Internal 'YrwControl': '<S324>:195' */
19328 /* Transition: '<S324>:196' */
19329 ((uint8_T *)ssGetDWork(S, 574))[0] = AFCS_MODEL1_IN_phiLcontrol;
19330
19331 /* Entry 'phiLcontrol': '<S324>:199' */
19332 _rtB->aplatL_b = 5;
19333 _rtB->apyawL_n = 1;
19334 _rtB->psdcmd_o = 0.0F;
19335 ((int32_T *)ssGetDWork(S, 321))[0] = 0;
19336 ((uint8_T *)ssGetDWork(S, 575))[0] = 1U;
19337 } else {
19338 /* During 'Climb': '<S324>:38' */
19339 if (((uint8_T *)ssGetDWork(S, 569))[0] ==
19340 AFCS_MODEL1_IN_PositionControl) {
19341 /* During 'PositionControl': '<S324>:29' */
19342 if (((int32_T *)ssGetDWork(S, 320))[0] == 5) {
19343 /* Transition: '<S324>:43' */
19344 /* Exit Internal 'PositionControl': '<S324>:29' */
19345 ((uint8_T *)ssGetDWork(S, 575))[0] = 0U;
19346
19347 /* Exit Internal 'XYPositionControl': '<S324>:131' */
19348 /* Exit Internal 'PositionControl1': '<S324>:135' */
19349 ((uint8_T *)ssGetDWork(S, 573))[0] =
19350 AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
19351
19352 /* Exit Internal 'YrwControl': '<S324>:195' */
19353 ((uint8_T *)ssGetDWork(S, 574))[0] =
19354 AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
19355 ((uint8_T *)ssGetDWork(S, 571))[0] =
19356 AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
19357 ((uint8_T *)ssGetDWork(S, 572))[0] = 0U;
19358 ((uint8_T *)ssGetDWork(S, 570))[0] = 0U;
19359 ((uint8_T *)ssGetDWork(S, 569))[0] = AFCS_MODEL1_IN_TransCruise;
19360
19361 /* Entry 'TransCruise': '<S324>:42' */
19362 _rtB->fmClimbtoCruise = 1;
19363 _rtB->K_j = 2;
19364 } else {
19365 _rtB->hcmd_eq = _rtB->href;
19366
19367 /* %Climb1FG=((drange<single(200.))&&(GPSFail==int8(0))); */
19368 ((int32_T *)ssGetDWork(S, 320))[0] = AFCS_MODEL1_addup_j
19369 ((_rtB->drange < 400.0F) && (_rtB->GPSFail == 0), 5, ((int32_T *)
19370 ssGetDWork(S, 320))[0]);
19371
19372 /* During 'AltitudeControl': '<S324>:115' */
19373 /* Transition: '<S324>:118' */
19374 if (_rtB->href - _rtB->mh_j < -50.0F) {
19375 /* Transition: '<S324>:116' */
19376 _rtB->aplonL_b = 3;
19377 _rtB->gamacmd_j = -5.0F;
19378 } else if (_rtB->href - _rtB->mh_j > 50.0F) {
19379 /* Transition: '<S324>:117' */
19380 _rtB->aplonL_b = 3;
19381 _rtB->gamacmd_j = 5.0F;
19382 } else {
19383 /* Transition: '<S324>:123' */
19384 _rtB->aplonL_b = 4;
19385 _rtB->gamacmd_j = 0.0F;
19386 }
19387
19388 /* During 'XYPositionControl': '<S324>:131' */
19389 /* During 'PositionControl1': '<S324>:135' */
19390 if (((uint8_T *)ssGetDWork(S, 573))[0] ==
19391 AFCS_MODEL1_IN_PhiControl) {
19392 /* During 'PhiControl': '<S324>:187' */
19393 if (_rtB->GPSFail == 0) {
19394 /* Transition: '<S324>:211' */
19395 ((uint8_T *)ssGetDWork(S, 573))[0] = AFCS_MODEL1_IN_YrwControl;
19396
19397 /* Entry Internal 'YrwControl': '<S324>:195' */
19398 /* Transition: '<S324>:196' */
19399 ((uint8_T *)ssGetDWork(S, 574))[0] =
19400 AFCS_MODEL1_IN_phiLcontrol;
19401
19402 /* Entry 'phiLcontrol': '<S324>:199' */
19403 _rtB->aplatL_b = 5;
19404 _rtB->apyawL_n = 1;
19405 _rtB->psdcmd_o = 0.0F;
19406 ((int32_T *)ssGetDWork(S, 321))[0] = 0;
19407 } else {
19408 /* Transition: '<S324>:189' */
19409 if (_rtB->mh_j > 30.0F) {
19410 /* Transition: '<S324>:191' */
19411 _rtB->phiLcmd_o = 30.0F;
19412 } else {
19413 /* Transition: '<S324>:194' */
19414 _rtB->phiLcmd_o = 0.0F;
19415 }
19416 }
19417 } else {
19418 /* During 'YrwControl': '<S324>:195' */
19419 if (_rtB->GPSFail == 1) {
19420 /* Transition: '<S324>:193' */
19421 /* Exit Internal 'YrwControl': '<S324>:195' */
19422 ((uint8_T *)ssGetDWork(S, 574))[0] =
19423 AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
19424 ((uint8_T *)ssGetDWork(S, 573))[0] = AFCS_MODEL1_IN_PhiControl;
19425
19426 /* Entry 'PhiControl': '<S324>:187' */
19427 _rtB->aplatL_b = 2;
19428 _rtB->apyawL_n = 1;
19429 } else if (((uint8_T *)ssGetDWork(S, 574))[0] ==
19430 AFCS_MODEL1_IN_YrwControl_o) {
19431 /* During 'YrwControl': '<S324>:205' */
19432 /* Transition: '<S324>:207' */
19433 if (_rtB->yrw - _rtB->yrwcmd_a > 50.0F) {
19434 /* Transition: '<S324>:209' */
19435 _rtB->aplatL_b = 4;
19436 _rtB->psdcmd_o = -10.0F;
19437 _rtB->phiLcmd_o = 0.0F;
19438 } else if (_rtB->yrw - _rtB->yrwcmd_a < -50.0F) {
19439 /* Transition: '<S324>:210' */
19440 _rtB->aplatL_b = 4;
19441 _rtB->psdcmd_o = 10.0F;
19442 _rtB->phiLcmd_o = 0.0F;
19443 } else {
19444 /* Transition: '<S324>:208' */
19445 _rtB->aplatL_b = 5;
19446 _rtB->yrwcmd_a = 0.0F;
19447 _rtB->psdcmd_o = 0.0F;
19448 _rtB->phiLcmd_o = 0.0F;
19449 }
19450 } else {
19451 /* During 'phiLcontrol': '<S324>:199' */
19452 if ((((int32_T *)ssGetDWork(S, 321))[0] == 5) && (_rtB->SWAUTO
19453 == 1)) {
19454 /* Transition: '<S324>:197' */
19455 ((uint8_T *)ssGetDWork(S, 574))[0] =
19456 AFCS_MODEL1_IN_YrwControl_o;
19457 } else {
19458 ((int32_T *)ssGetDWork(S, 321))[0] = AFCS_MODEL1_addup_j
19459 ((real32_T)fabs(AFCS_MODEL1_res180(_rtB->psdcmd_o -
19460 _rtB->psd)) < 15.0F, 5, ((int32_T *)ssGetDWork(S, 321))
19461 [0]);
19462
19463 /* Transition: '<S324>:201' */
19464 if (AFCS_MODEL1_res180(_rtB->psdcmd_o - _rtB->psd) > 20.0F)
19465 {
19466 /* Transition: '<S324>:202' */
19467 _rtB->aplatL_b = 2;
19468 _rtB->phiLcmd_o = 30.0F;
19469 } else {
19470 if (AFCS_MODEL1_res180(_rtB->psdcmd_o - _rtB->psd) <
19471 -20.0F) {
19472 /* Transition: '<S324>:204' */
19473 _rtB->aplatL_b = 2;
19474 _rtB->phiLcmd_o = -30.0F;
19475 } else {
19476 /* Transition: '<S324>:203' */
19477 _rtB->aplatL_b = 4;
19478 _rtB->psdcmd_o = 0.0F;
19479 _rtB->phiLcmd_o = 0.0F;
19480 }
19481 }
19482 }
19483 }
19484 }
19485
19486 /* During 'ThrustControl': '<S324>:175' */
19487 /* Transition: '<S324>:177' */
19488 if (_rtB->Vc_p > 45.0F) {
19489 /* Transition: '<S324>:182' */
19490 _rtB->apthL_fb = 3;
19491 _rtB->vtcmd_h1 = 45.0F;
19492 } else {
19493 /* Transition: '<S324>:183' */
19494 }
19495 }
19496 } else {
19497 /* During 'TransCruise': '<S324>:42' */
19498 }
19499 }
19500
19501 /* End of Chart: '<S314>/Climb2' */
19502 }
19503
19504 if (1) {
19505 /* SignalConversion: '<S314>/Signal Conversion' */
19506 _rtB->configure = _rtB->configure_o;
19507
19508 /* SignalConversion: '<S314>/Signal Conversion' */
19509 _rtB->vtcmd = _rtB->vtcmd_h1;
19510
19511 /* SignalConversion: '<S314>/Signal Conversion' */
19512 _rtB->nzcmd = _rtB->nzcmd_o;
19513
19514 /* SignalConversion: '<S314>/Signal Conversion' */
19515 _rtB->nycmd = _rtB->nycmd_n;
19516
19517 /* SignalConversion: '<S314>/Signal Conversion' */
19518 _rtB->phiLcmd = _rtB->phiLcmd_o;
19519
19520 /* SignalConversion: '<S314>/Signal Conversion' */
19521 _rtB->thetaLcmd = _rtB->thetaLcmd_g;
19522
19523 /* SignalConversion: '<S314>/Signal Conversion' */
19524 _rtB->psiLcmd = _rtB->psiLcmd_j;
19525
19526 /* SignalConversion: '<S314>/Signal Conversion' */
19527 _rtB->gamacmd = _rtB->gamacmd_j;
19528
19529 /* SignalConversion: '<S314>/Signal Conversion' */
19530 _rtB->hcmd = _rtB->hcmd_eq;
19531
19532 /* SignalConversion: '<S314>/Signal Conversion' */
19533 _rtB->psdcmd = _rtB->psdcmd_o;
19534
19535 /* SignalConversion: '<S314>/Signal Conversion' */
19536 _rtB->yrwcmd = _rtB->yrwcmd_a;
19537
19538 /* SignalConversion: '<S314>/Signal Conversion' */
19539 _rtB->apthL = _rtB->apthL_fb;
19540
19541 /* SignalConversion: '<S314>/Signal Conversion' */
19542 _rtB->apcheck = _rtB->apcheck_c;
19543
19544 /* SignalConversion: '<S314>/Signal Conversion' */
19545 _rtB->apfix = _rtB->apfix_h5;
19546
19547 /* SignalConversion: '<S314>/Signal Conversion' */
19548 _rtB->K = _rtB->K_j;
19549
19550 /* SignalConversion: '<S314>/Signal Conversion' */
19551 _rtB->StartEmNav = _rtB->StartEmNav_a2;
19552
19553 /* SignalConversion: '<S314>/Signal Conversion' */
19554 _rtB->aplonL = _rtB->aplonL_b;
19555
19556 /* SignalConversion: '<S314>/Signal Conversion' */
19557 _rtB->aplatL = _rtB->aplatL_b;
19558
19559 /* SignalConversion: '<S314>/Signal Conversion' */
19560 _rtB->apyawL = _rtB->apyawL_n;
19561
19562 /* SignalConversion: '<S314>/Signal Conversion' */
19563 _rtB->apsbL = _rtB->apsbL_ex;
19564
19565 /* SignalConversion: '<S314>/Signal Conversion' */
19566 _rtB->apnwsL = _rtB->apnwsL_ku;
19567
19568 /* SignalConversion: '<S314>/Signal Conversion' */
19569 _rtB->apbrkL = _rtB->apbrkL_b;
19570
19571 /* SignalConversion: '<S314>/Signal Conversion' */
19572 _rtB->thrustcmd = _rtB->thrustcmd_k;
19573 }
19574
19575 /* End of Outputs for SubSystem: '<S31>/Climb' */
19576 break;
19577
19578 case 3:
19579 /* Outputs for IfAction SubSystem: '<S31>/Cruise' incorporates:
19580 * ActionPort: '<S315>/Action Port'
19581 */
19582 if (ssIsSampleHit(S, 1, 0)) {
19583 /* Chart: '<S315>/Cruise' incorporates:
19584 * Constant: '<S17>/Constant15'
19585 * Constant: '<S17>/Constant16'
19586 * Constant: '<S17>/Constant6'
19587 * Inport: '<Root>/AT_MODE'
19588 * Inport: '<Root>/CMD_PhiC'
19589 * Inport: '<Root>/DV'
19590 * Inport: '<Root>/YD_MODE'
19591 */
19592 /* Gateway: AFCS_MODEL1/GNC/logic/Cruise/Cruise */
19593 /* During: AFCS_MODEL1/GNC/logic/Cruise/Cruise */
19594 if (((uint8_T *)ssGetDWork(S, 557))[0] == 0U) {
19595 /* Entry: AFCS_MODEL1/GNC/logic/Cruise/Cruise */
19596 ((uint8_T *)ssGetDWork(S, 557))[0] = 1U;
19597
19598 /* Entry Internal: AFCS_MODEL1/GNC/logic/Cruise/Cruise */
19599 ((uint8_T *)ssGetDWork(S, 558))[0] = AFCS_MODEL1_IN_Cruise;
19600
19601 /* Entry 'Cruise': '<S325>:38' */
19602 _rtB->configure_j = 1;
19603
19604 /* 滚转角控制模式 */
19605 _rtB->apyawL_mn = 1;
19606 _rtB->apsbL_g = 1;
19607 _rtB->apnwsL_o = 1;
19608 _rtB->apbrkL_a = 1;
19609 _rtB->thrustcmd_f = 0.0F;
19610 _rtB->vtcmd_l = 40.0F;
19611 _rtB->nzcmd_n = 0.0F;
19612 _rtB->nycmd_l = 0.0F;
19613 _rtB->thetaLcmd_a = 0.0F;
19614 _rtB->psiLcmd_e = 0.0F;
19615 _rtB->psdcmd_ik = 0.0F;
19616 _rtB->yrwcmd_f = 0.0F;
19617 _rtB->apfix_h = 0;
19618 _rtB->apcheck_b = 0;
19619 _rtB->fmCruisetoAL = 0;
19620 _rtB->K_b = 2;
19621 _rtB->StartEmNav_l = 0;
19622
19623 /* Entry Internal 'Cruise': '<S325>:38' */
19624 ((uint8_T *)ssGetDWork(S, 560))[0] = 1U;
19625
19626 /* Entry Internal 'AltitudeControl': '<S325>:773' */
19627 /* Transition: '<S325>:808' */
19628 ((uint8_T *)ssGetDWork(S, 559))[0] = AFCS_MODEL1_IN_AltControl;
19629
19630 /* Entry Internal 'AltControl': '<S325>:807' */
19631 /* Transition: '<S325>:814' */
19632 ((uint8_T *)ssGetDWork(S, 561))[0] = AFCS_MODEL1_IN_Hcontrol;
19633
19634 /* Entry 'Hcontrol': '<S325>:829' */
19635 _rtB->aplonL_i = 4;
19636 _rtB->hcmd_e = _rtB->Saturation2_g;
19637 _rtB->gamacmd_pg = 0.0F;
19638 ((uint8_T *)ssGetDWork(S, 563))[0] = 1U;
19639
19640 /* Entry 'PhiControl': '<S325>:780' */
19641 _rtB->CutPiontFG = 0;
19642 _rtB->TargetPiont = 0;
19643
19644 /* Entry Internal 'PhiControl': '<S325>:780' */
19645 /* Transition: '<S325>:781' */
19646 ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_PhiControl1;
19647
19648 /* Entry 'PhiControl1': '<S325>:785' */
19649 _rtB->aplatL_i = 2;
19650
19651 /* 滚转角控制模式 */
19652 _rtB->phiLcmd_n = 0.0F;
19653 ((uint8_T *)ssGetDWork(S, 565))[0] = 1U;
19654
19655 /* Entry 'ThrustControl': '<S325>:796' */
19656 /* Entry Internal 'ThrustControl': '<S325>:796' */
19657 /* Transition: '<S325>:797' */
19658 ((uint8_T *)ssGetDWork(S, 564))[0] = AFCS_MODEL1_IN_thrustControl1;
19659
19660 /* Entry 'thrustControl1': '<S325>:803' */
19661 _rtB->apthL_k = 3;
19662 ((uint8_T *)ssGetDWork(S, 566))[0] = 1U;
19663
19664 /* Entry 'fmphaseTranse': '<S325>:769' */
19665 ((int32_T *)ssGetDWork(S, 319))[0] = 0;
19666 } else {
19667 /* During 'Cruise': '<S325>:38' */
19668 _rtB->hcmd_e = _rtB->Saturation2_g;
19669 _rtB->K_b = _rtB->y_p;
19670
19671 /* During 'AltitudeControl': '<S325>:773' */
19672 if (((uint8_T *)ssGetDWork(S, 559))[0] == AFCS_MODEL1_IN_AltControl) {
19673 /* During 'AltControl': '<S325>:807' */
19674 if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] == 1) {
19675 /* Transition: '<S325>:823' */
19676 /* Exit Internal 'AltControl': '<S325>:807' */
19677 ((uint8_T *)ssGetDWork(S, 561))[0] =
19678 AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
19679 ((uint8_T *)ssGetDWork(S, 559))[0] = AFCS_MODEL1_IN_HdotControl;
19680
19681 /* Entry 'HdotControl': '<S325>:817' */
19682 _rtB->aplonL_i = 5;
19683 } else {
19684 switch (((uint8_T *)ssGetDWork(S, 561))[0]) {
19685 case AFCS_MODEL1_IN_Hcontrol:
19686 /* During 'Hcontrol': '<S325>:829' */
19687 if (_rtB->Saturation2_g - _rtB->mh_j > 50.0F) {
19688 /* Transition: '<S325>:815' */
19689 ((uint8_T *)ssGetDWork(S, 561))[0] =
19690 AFCS_MODEL1_IN_gamacontrol;
19691
19692 /* Entry 'gamacontrol': '<S325>:827' */
19693 _rtB->aplonL_i = 3;
19694 _rtB->gamacmd_pg = 5.0F;
19695 } else {
19696 if (_rtB->Saturation2_g - _rtB->mh_j < -50.0F) {
19697 /* Transition: '<S325>:816' */
19698 ((uint8_T *)ssGetDWork(S, 561))[0] =
19699 AFCS_MODEL1_IN_gamacontrol1;
19700
19701 /* Entry 'gamacontrol1': '<S325>:828' */
19702 _rtB->aplonL_i = 3;
19703 _rtB->gamacmd_pg = -5.0F;
19704 }
19705 }
19706 break;
19707
19708 case AFCS_MODEL1_IN_gamacontrol:
19709 /* During 'gamacontrol': '<S325>:827' */
19710 if ((real32_T)fabs(_rtB->Saturation2_g - _rtB->mh_j) < 50.0F) {
19711 /* Transition: '<S325>:831' */
19712 ((uint8_T *)ssGetDWork(S, 561))[0] = AFCS_MODEL1_IN_Hcontrol;
19713
19714 /* Entry 'Hcontrol': '<S325>:829' */
19715 _rtB->aplonL_i = 4;
19716 _rtB->gamacmd_pg = 0.0F;
19717 }
19718 break;
19719
19720 default:
19721 /* During 'gamacontrol1': '<S325>:828' */
19722 if ((real32_T)fabs(_rtB->Saturation2_g - _rtB->mh_j) < 50.0F) {
19723 /* Transition: '<S325>:830' */
19724 ((uint8_T *)ssGetDWork(S, 561))[0] = AFCS_MODEL1_IN_Hcontrol;
19725
19726 /* Entry 'Hcontrol': '<S325>:829' */
19727 _rtB->aplonL_i = 4;
19728 _rtB->gamacmd_pg = 0.0F;
19729 }
19730 break;
19731 }
19732 }
19733 } else {
19734 /* During 'HdotControl': '<S325>:817' */
19735 if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] != 1) {
19736 /* Transition: '<S325>:824' */
19737 ((uint8_T *)ssGetDWork(S, 559))[0] = AFCS_MODEL1_IN_AltControl;
19738
19739 /* Entry Internal 'AltControl': '<S325>:807' */
19740 /* Transition: '<S325>:814' */
19741 ((uint8_T *)ssGetDWork(S, 561))[0] = AFCS_MODEL1_IN_Hcontrol;
19742
19743 /* Entry 'Hcontrol': '<S325>:829' */
19744 _rtB->aplonL_i = 4;
19745 _rtB->gamacmd_pg = 0.0F;
19746 } else {
19747 /* gamacmd=single(0); */
19748 /* gamacmd=asind(CMD_Hdotc/vt); */
19749 }
19750 }
19751
19752 /* During 'PhiControl': '<S325>:780' */
19753 switch (((uint8_T *)ssGetDWork(S, 562))[0]) {
19754 case AFCS_MODEL_IN_GPSFailPhiControl:
19755 /* During 'GPSFailPhiControl': '<S325>:786' */
19756 if (_rtB->GPSFail == 0) {
19757 /* Transition: '<S325>:783' */
19758 ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_PhiControl1;
19759
19760 /* Entry 'PhiControl1': '<S325>:785' */
19761 _rtB->aplatL_i = 2;
19762
19763 /* 滚转角控制模式 */
19764 _rtB->phiLcmd_n = 0.0F;
19765 }
19766 break;
19767
19768 case AFCS_MODEL1_IN_PhiControl1:
19769 /* During 'PhiControl1': '<S325>:785' */
19770 if (_rtB->GPSFail == 1) {
19771 /* Transition: '<S325>:782' */
19772 ((uint8_T *)ssGetDWork(S, 562))[0] =
19773 AFCS_MODEL_IN_GPSFailPhiControl;
19774
19775 /* Entry 'GPSFailPhiControl': '<S325>:786' */
19776 _rtB->aplatL_i = 2;
19777 _rtB->apyawL_mn = 1;
19778 _rtB->phiLcmd_n = 30.0F;
19779 } else if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] ==
19780 1) {
19781 /* Transition: '<S325>:791' */
19782 ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_YD_MODE;
19783
19784 /* Entry 'YD_MODE': '<S325>:790' */
19785 _rtB->aplatL_i = 2;
19786 _rtB->apyawL_mn = 1;
19787 } else if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] ==
19788 2) {
19789 /* Transition: '<S325>:860' */
19790 i_1 = _rtB->CutPiontFG + 1;
19791 if (i_1 > 127) {
19792 i_1 = 127;
19793 }
19794
19795 _rtB->CutPiontFG = (int8_T)i_1;
19796
19797 /* 复飞 */
19798 _rtB->TargetPiont = 2;
19799 ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_ReFly_MODE;
19800
19801 /* Entry 'ReFly_MODE': '<S325>:858' */
19802 _rtB->aplatL_i = 2;
19803
19804 /* 滚转角控制模式 */
19805 _rtB->phiLcmd_n = 0.0F;
19806 } else {
19807 _rtB->phiLcmd_n = _rtB->Saturation_h;
19808 }
19809 break;
19810
19811 case AFCS_MODEL1_IN_ReFly_MODE:
19812 /* During 'ReFly_MODE': '<S325>:858' */
19813 if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] == 1) {
19814 /* Transition: '<S325>:859' */
19815 ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_YD_MODE;
19816
19817 /* Entry 'YD_MODE': '<S325>:790' */
19818 _rtB->aplatL_i = 2;
19819 _rtB->apyawL_mn = 1;
19820 } else if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] ==
19821 0) {
19822 /* Transition: '<S325>:861' */
19823 ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_PhiControl1;
19824
19825 /* Entry 'PhiControl1': '<S325>:785' */
19826 _rtB->aplatL_i = 2;
19827
19828 /* 滚转角控制模式 */
19829 _rtB->phiLcmd_n = 0.0F;
19830 } else {
19831 _rtB->phiLcmd_n = _rtB->Saturation_h;
19832 }
19833 break;
19834
19835 default:
19836 /* During 'YD_MODE': '<S325>:790' */
19837 if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] == 0) {
19838 /* Transition: '<S325>:847' */
19839 i_1 = _rtB->CutPiontFG + 1;
19840 if (i_1 > 127) {
19841 i_1 = 127;
19842 }
19843
19844 _rtB->CutPiontFG = (int8_T)i_1;
19845
19846 /* 由引导模式切回自主,需要切一次航点 */
19847 _rtB->TargetPiont = _rtB->y_p;
19848 ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_PhiControl1;
19849
19850 /* Entry 'PhiControl1': '<S325>:785' */
19851 _rtB->aplatL_i = 2;
19852
19853 /* 滚转角控制模式 */
19854 _rtB->phiLcmd_n = 0.0F;
19855 } else if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 0))[0] ==
19856 2) {
19857 /* Transition: '<S325>:855' */
19858 i_1 = _rtB->CutPiontFG + 1;
19859 if (i_1 > 127) {
19860 i_1 = 127;
19861 }
19862
19863 _rtB->CutPiontFG = (int8_T)i_1;
19864
19865 /* 复飞 */
19866 _rtB->TargetPiont = 2;
19867 ((uint8_T *)ssGetDWork(S, 562))[0] = AFCS_MODEL1_IN_ReFly_MODE;
19868
19869 /* Entry 'ReFly_MODE': '<S325>:858' */
19870 _rtB->aplatL_i = 2;
19871
19872 /* 滚转角控制模式 */
19873 _rtB->phiLcmd_n = 0.0F;
19874 } else {
19875 _rtB->phiLcmd_n = *((const real32_T **)ssGetInputPortSignalPtrs(S,
19876 3))[0];
19877 }
19878 break;
19879 }
19880
19881 /* During 'ThrustControl': '<S325>:796' */
19882 if (((uint8_T *)ssGetDWork(S, 564))[0] == AFCS_MODEL1_IN_AT_MODE) {
19883 /* During 'AT_MODE': '<S325>:802' */
19884 if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 1))[0] == 0) {
19885 /* Transition: '<S325>:799' */
19886 ((uint8_T *)ssGetDWork(S, 564))[0] = AFCS_MODEL1_IN_thrustControl1;
19887
19888 /* Entry 'thrustControl1': '<S325>:803' */
19889 _rtB->apthL_k = 3;
19890 } else {
19891 _rtB->vtcmd_l = *((const real32_T **)ssGetInputPortSignalPtrs(S, 2))
19892 [0];
19893 }
19894 } else {
19895 /* During 'thrustControl1': '<S325>:803' */
19896 if (*((const int8_T **)ssGetInputPortSignalPtrs(S, 1))[0] == 1) {
19897 /* Transition: '<S325>:798' */
19898 ((uint8_T *)ssGetDWork(S, 564))[0] = AFCS_MODEL1_IN_AT_MODE;
19899
19900 /* Entry 'AT_MODE': '<S325>:802' */
19901 _rtB->apthL_k = 3;
19902 } else {
19903 _rtB->vtcmd_l = 40.0F;
19904 }
19905 }
19906
19907 /* During 'fmphaseTranse': '<S325>:769' */
19908 /* %Cruise1FG=(VxdNest==int32(7000)); */
19909 i_1 = _rtB->Vxd1;
19910 if (_rtB->Vxd1 > 127) {
19911 i_1 = 127;
19912 } else {
19913 if (_rtB->Vxd1 < -128) {
19914 i_1 = -128;
19915 }
19916 }
19917
19918 ((int32_T *)ssGetDWork(S, 319))[0] = AFCS_MODEL1_addup_o(_rtB->K_b ==
19919 i_1, 5, ((int32_T *)ssGetDWork(S, 319))[0]);
19920 if ((((int32_T *)ssGetDWork(S, 319))[0] == 5) || (_rtB->EnLowFlag == 1)
19921 || (_rtB->EmlandFg == 1)) {
19922 /* Transition: '<S325>:770' */
19923 _rtB->fmCruisetoAL = 1;
19924 }
19925 }
19926
19927 /* End of Chart: '<S315>/Cruise' */
19928 }
19929
19930 if (1) {
19931 /* SignalConversion: '<S315>/Signal Conversion' */
19932 _rtB->configure = _rtB->configure_j;
19933
19934 /* SignalConversion: '<S315>/Signal Conversion' */
19935 _rtB->vtcmd = _rtB->vtcmd_l;
19936
19937 /* SignalConversion: '<S315>/Signal Conversion' */
19938 _rtB->nzcmd = _rtB->nzcmd_n;
19939
19940 /* SignalConversion: '<S315>/Signal Conversion' */
19941 _rtB->nycmd = _rtB->nycmd_l;
19942
19943 /* SignalConversion: '<S315>/Signal Conversion' */
19944 _rtB->phiLcmd = _rtB->phiLcmd_n;
19945
19946 /* SignalConversion: '<S315>/Signal Conversion' */
19947 _rtB->thetaLcmd = _rtB->thetaLcmd_a;
19948
19949 /* SignalConversion: '<S315>/Signal Conversion' */
19950 _rtB->psiLcmd = _rtB->psiLcmd_e;
19951
19952 /* SignalConversion: '<S315>/Signal Conversion' */
19953 _rtB->gamacmd = _rtB->gamacmd_pg;
19954
19955 /* SignalConversion: '<S315>/Signal Conversion' */
19956 _rtB->hcmd = _rtB->hcmd_e;
19957
19958 /* SignalConversion: '<S315>/Signal Conversion' */
19959 _rtB->psdcmd = _rtB->psdcmd_ik;
19960
19961 /* SignalConversion: '<S315>/Signal Conversion' */
19962 _rtB->yrwcmd = _rtB->yrwcmd_f;
19963
19964 /* SignalConversion: '<S315>/Signal Conversion' */
19965 _rtB->apthL = _rtB->apthL_k;
19966
19967 /* SignalConversion: '<S315>/Signal Conversion' */
19968 _rtB->apcheck = _rtB->apcheck_b;
19969
19970 /* SignalConversion: '<S315>/Signal Conversion' */
19971 _rtB->apfix = _rtB->apfix_h;
19972
19973 /* SignalConversion: '<S315>/Signal Conversion' */
19974 _rtB->K = _rtB->K_b;
19975
19976 /* SignalConversion: '<S315>/Signal Conversion' */
19977 _rtB->StartEmNav = _rtB->StartEmNav_l;
19978
19979 /* SignalConversion: '<S315>/Signal Conversion' */
19980 _rtB->aplonL = _rtB->aplonL_i;
19981
19982 /* SignalConversion: '<S315>/Signal Conversion' */
19983 _rtB->aplatL = _rtB->aplatL_i;
19984
19985 /* SignalConversion: '<S315>/Signal Conversion' */
19986 _rtB->apyawL = _rtB->apyawL_mn;
19987
19988 /* SignalConversion: '<S315>/Signal Conversion' */
19989 _rtB->apsbL = _rtB->apsbL_g;
19990
19991 /* SignalConversion: '<S315>/Signal Conversion' */
19992 _rtB->apnwsL = _rtB->apnwsL_o;
19993
19994 /* SignalConversion: '<S315>/Signal Conversion' */
19995 _rtB->apbrkL = _rtB->apbrkL_a;
19996
19997 /* SignalConversion: '<S315>/Signal Conversion' */
19998 _rtB->thrustcmd = _rtB->thrustcmd_f;
19999 }
20000
20001 /* End of Outputs for SubSystem: '<S31>/Cruise' */
20002 break;
20003
20004 case 4:
20005 /* Outputs for IfAction SubSystem: '<S31>/Approach' incorporates:
20006 * ActionPort: '<S313>/Action Port'
20007 */
20008 if (1) {
20009 /* Sum: '<S313>/Sum' incorporates:
20010 * Constant: '<S313>/Constant4'
20011 */
20012 _rtB->Sum_ov = (int8_T)(_rtB->NumPoint - 3);
20013 }
20014
20015 if (ssIsSampleHit(S, 1, 0)) {
20016 /* Chart: '<S313>/Approach2' incorporates:
20017 * Constant: '<S17>/Constant10'
20018 * Constant: '<S17>/Constant15'
20019 * Constant: '<S17>/Constant16'
20020 * Constant: '<S17>/Constant6'
20021 */
20022 /* Gateway: AFCS_MODEL1/GNC/logic/Approach/Approach2 */
20023 /* During: AFCS_MODEL1/GNC/logic/Approach/Approach2 */
20024 if (((uint8_T *)ssGetDWork(S, 547))[0] == 0U) {
20025 /* Entry: AFCS_MODEL1/GNC/logic/Approach/Approach2 */
20026 ((uint8_T *)ssGetDWork(S, 547))[0] = 1U;
20027
20028 /* Entry Internal: AFCS_MODEL1/GNC/logic/Approach/Approach2 */
20029 ((uint8_T *)ssGetDWork(S, 548))[0] = AFCS_MODEL1_IN_Approach;
20030
20031 /* Entry 'Approach': '<S323>:38' */
20032 _rtB->configure_c = 1;
20033 _rtB->apthL_f = 3;
20034 _rtB->aplonL_o1 = 4;
20035 _rtB->apsbL_p = 1;
20036 _rtB->apnwsL_i = 1;
20037 _rtB->apbrkL_ot = 1;
20038 _rtB->thrustcmd_e = 0.0F;
20039 _rtB->vtcmd_h = 40.0F;
20040 _rtB->nzcmd_b = 0.0F;
20041 _rtB->nycmd_p = 0.0F;
20042 _rtB->phiLcmd_l4 = 0.0F;
20043 _rtB->thetaLcmd_l = 0.0F;
20044 _rtB->psiLcmd_n = 0.0F;
20045 _rtB->gamacmd_c = 0.0F;
20046 _rtB->hcmd_g = _rtB->href;
20047 _rtB->yrwcmd_i = 0.0F;
20048 _rtB->apfix_e = 0;
20049 _rtB->apcheck_h = 0;
20050 _rtB->fmALtoLand = 0;
20051 _rtB->K_m = _rtB->Sum_ov;
20052 _rtB->StartEmNav_o = 0;
20053
20054 /* Entry Internal 'Approach': '<S323>:38' */
20055 /* Transition: '<S323>:95' */
20056 ((uint8_T *)ssGetDWork(S, 549))[0] = AFCS_MODEL1_IN_FlightMode;
20057
20058 /* Entry 'FlightMode': '<S323>:287' */
20059 ((int32_T *)ssGetDWork(S, 313))[0] = 0;
20060
20061 /* Entry Internal 'FlightMode': '<S323>:287' */
20062 ((uint8_T *)ssGetDWork(S, 550))[0] = 1U;
20063 ((uint8_T *)ssGetDWork(S, 552))[0] = 1U;
20064
20065 /* Entry 'XYPositionControl': '<S323>:452' */
20066 ((int32_T *)ssGetDWork(S, 317))[0] = 0;
20067
20068 /* Entry Internal 'XYPositionControl': '<S323>:452' */
20069 /* Transition: '<S323>:487' */
20070 ((uint8_T *)ssGetDWork(S, 551))[0] = AFCS_MODEL1_IN_NormYrwControl;
20071
20072 /* Entry 'NormYrwControl': '<S323>:454' */
20073 /* Entry Internal 'NormYrwControl': '<S323>:454' */
20074 /* Transition: '<S323>:455' */
20075 ((uint8_T *)ssGetDWork(S, 553))[0] = AFCS_MODEL1_IN_YrwControl1;
20076
20077 /* Entry Internal 'YrwControl1': '<S323>:550' */
20078 /* Transition: '<S323>:551' */
20079 ((uint8_T *)ssGetDWork(S, 554))[0] = AFCS_MODEL1_IN_phiLcontrol;
20080
20081 /* Entry 'phiLcontrol': '<S323>:554' */
20082 _rtB->aplatL_a = 5;
20083 _rtB->apyawL_fh = 1;
20084 _rtB->psdcmd_g = 0.0F;
20085 ((int32_T *)ssGetDWork(S, 314))[0] = 0;
20086 } else {
20087 /* During 'Approach': '<S323>:38' */
20088 _rtB->hcmd_g = _rtB->href;
20089 _rtB->vtcmd_h = 40.0F;
20090 if (((uint8_T *)ssGetDWork(S, 549))[0] == AFCS_MODEL1_IN_FlightMode) {
20091 /* During 'FlightMode': '<S323>:287' */
20092 if (((int32_T *)ssGetDWork(S, 313))[0] == 5) {
20093 /* Transition: '<S323>:43' */
20094 /* Exit Internal 'FlightMode': '<S323>:287' */
20095 /* Exit Internal 'XYPositionControl': '<S323>:452' */
20096 /* Exit Internal 'EmYrwControl': '<S323>:493' */
20097 ((uint8_T *)ssGetDWork(S, 555))[0] =
20098 AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
20099
20100 /* Exit Internal 'LevelPosition': '<S323>:567' */
20101 ((uint8_T *)ssGetDWork(S, 556))[0] =
20102 AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
20103 ((uint8_T *)ssGetDWork(S, 551))[0] =
20104 AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
20105
20106 /* Exit Internal 'NormYrwControl': '<S323>:454' */
20107 ((uint8_T *)ssGetDWork(S, 553))[0] =
20108 AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
20109
20110 /* Exit Internal 'YrwControl1': '<S323>:550' */
20111 ((uint8_T *)ssGetDWork(S, 554))[0] =
20112 AFCS_MODEL1_IN_NO_ACTIVE_CHILD;
20113 ((uint8_T *)ssGetDWork(S, 552))[0] = 0U;
20114 ((uint8_T *)ssGetDWork(S, 550))[0] = 0U;
20115 ((uint8_T *)ssGetDWork(S, 549))[0] = AFCS_MODEL1_IN_TransLand;
20116
20117 /* Entry 'TransLand': '<S323>:42' */
20118 _rtB->fmALtoLand = 1;
20119 i_1 = _rtB->Sum_ov + 1;
20120 if (i_1 > 127) {
20121 i_1 = 127;
20122 }
20123
20124 _rtB->K_m = (int8_T)i_1;
20125 } else {
20126 ((int32_T *)ssGetDWork(S, 313))[0] = AFCS_MODEL1_addup_f
20127 ((_rtB->drange < 400.0F) && ((real32_T)fabs(_rtB->psd) < 90.0F) &&
20128 ((real32_T)fabs(_rtB->href - _rtB->mh_j) < 100.0F) &&
20129 ((real32_T)fabs(_rtB->yrw - _rtB->yrwcmd_i) < 100.0F) &&
20130 (_rtB->SWAUTO == 1) && (_rtB->StartEmNav_o == 0), 5, ((int32_T *)
20131 ssGetDWork(S, 313))[0]);
20132
20133 /* During 'AltitudeControl': '<S323>:469' */
20134 /* Transition: '<S323>:471' */
20135 if (_rtB->hcmd_g - _rtB->mh_j < -50.0F) {
20136 /* Transition: '<S323>:474' */
20137 _rtB->aplonL_o1 = 3;
20138 _rtB->gamacmd_c = -5.0F;
20139 } else if (_rtB->hcmd_g - _rtB->mh_j > 50.0F) {
20140 /* Transition: '<S323>:473' */
20141 _rtB->aplonL_o1 = 3;
20142 _rtB->gamacmd_c = 5.0F;
20143 } else {
20144 /* Transition: '<S323>:472' */
20145 _rtB->aplonL_o1 = 4;
20146 _rtB->gamacmd_c = 0.0F;
20147 }
20148
20149 AFCS_MODEL1_XYPositionControl(S);
20150 }
20151 } else {
20152 /* During 'TransLand': '<S323>:42' */
20153 }
20154 }
20155
20156 /* End of Chart: '<S313>/Approach2' */
20157 }
20158
20159 if (1) {
20160 /* SignalConversion: '<S313>/Signal Conversion' */
20161 _rtB->configure = _rtB->configure_c;
20162
20163 /* SignalConversion: '<S313>/Signal Conversion' */
20164 _rtB->vtcmd = _rtB->vtcmd_h;
20165
20166 /* SignalConversion: '<S313>/Signal Conversion' */
20167 _rtB->nzcmd = _rtB->nzcmd_b;
20168
20169 /* SignalConversion: '<S313>/Signal Conversion' */
20170 _rtB->nycmd = _rtB->nycmd_p;
20171
20172 /* SignalConversion: '<S313>/Signal Conversion' */
20173 _rtB->phiLcmd = _rtB->phiLcmd_l4;
20174
20175 /* SignalConversion: '<S313>/Signal Conversion' */
20176 _rtB->thetaLcmd = _rtB->thetaLcmd_l;
20177
20178 /* SignalConversion: '<S313>/Signal Conversion' */
20179 _rtB->psiLcmd = _rtB->psiLcmd_n;
20180
20181 /* SignalConversion: '<S313>/Signal Conversion' */
20182 _rtB->gamacmd = _rtB->gamacmd_c;
20183
20184 /* SignalConversion: '<S313>/Signal Conversion' */
20185 _rtB->hcmd = _rtB->hcmd_g;
20186
20187 /* SignalConversion: '<S313>/Signal Conversion' */
20188 _rtB->psdcmd = _rtB->psdcmd_g;
20189
20190 /* SignalConversion: '<S313>/Signal Conversion' */
20191 _rtB->yrwcmd = _rtB->yrwcmd_i;
20192
20193 /* SignalConversion: '<S313>/Signal Conversion' */
20194 _rtB->apthL = _rtB->apthL_f;
20195
20196 /* SignalConversion: '<S313>/Signal Conversion' */
20197 _rtB->apcheck = _rtB->apcheck_h;
20198
20199 /* SignalConversion: '<S313>/Signal Conversion' */
20200 _rtB->apfix = _rtB->apfix_e;
20201
20202 /* SignalConversion: '<S313>/Signal Conversion' */
20203 _rtB->K = _rtB->K_m;
20204
20205 /* SignalConversion: '<S313>/Signal Conversion' */
20206 _rtB->StartEmNav = _rtB->StartEmNav_o;
20207
20208 /* SignalConversion: '<S313>/Signal Conversion' */
20209 _rtB->aplonL = _rtB->aplonL_o1;
20210
20211 /* SignalConversion: '<S313>/Signal Conversion' */
20212 _rtB->aplatL = _rtB->aplatL_a;
20213
20214 /* SignalConversion: '<S313>/Signal Conversion' */
20215 _rtB->apyawL = _rtB->apyawL_fh;
20216
20217 /* SignalConversion: '<S313>/Signal Conversion' */
20218 _rtB->apsbL = _rtB->apsbL_p;
20219
20220 /* SignalConversion: '<S313>/Signal Conversion' */
20221 _rtB->apnwsL = _rtB->apnwsL_i;
20222
20223 /* SignalConversion: '<S313>/Signal Conversion' */
20224 _rtB->apbrkL = _rtB->apbrkL_ot;
20225
20226 /* SignalConversion: '<S313>/Signal Conversion' */
20227 _rtB->thrustcmd = _rtB->thrustcmd_e;
20228 }
20229
20230 /* End of Outputs for SubSystem: '<S31>/Approach' */
20231 break;
20232
20233 case 5:
20234 /* Outputs for IfAction SubSystem: '<S31>/Landing' incorporates:
20235 * ActionPort: '<S316>/Action Port'
20236 */
20237 if (1) {
20238 /* Sum: '<S316>/Sum' incorporates:
20239 * Constant: '<S316>/Constant4'
20240 */
20241 _rtB->Sum_mn = (int8_T)(_rtB->NumPoint - 2);
20242 }
20243
20244 if (ssIsSampleHit(S, 1, 0)) {
20245 /* Chart: '<S316>/Landing2' incorporates:
20246 * Constant: '<S17>/Constant6'
20247 */
20248 /* Gateway: AFCS_MODEL1/GNC/logic/Landing/Landing2 */
20249 /* During: AFCS_MODEL1/GNC/logic/Landing/Landing2 */
20250 if (((uint8_T *)ssGetDWork(S, 532))[0] == 0U) {
20251 /* Entry: AFCS_MODEL1/GNC/logic/Landing/Landing2 */
20252 ((uint8_T *)ssGetDWork(S, 532))[0] = 1U;
20253
20254 /* Entry Internal: AFCS_MODEL1/GNC/logic/Landing/Landing2 */
20255 ((uint8_T *)ssGetDWork(S, 533))[0] = AFCS_MODEL1_IN_Landing;
20256
20257 /* Entry 'Landing': '<S326>:38' */
20258 _rtB->configure_gv = 1;
20259 _rtB->apthL_e = 3;
20260 _rtB->aplonL_f = 4;
20261 _rtB->apyawL_m = 1;
20262 _rtB->apsbL_l = 1;
20263 _rtB->apnwsL_m = 1;
20264 _rtB->apbrkL_n = 1;
20265 _rtB->thrustcmd_ho = 0.0F;
20266 _rtB->vtcmd_f = 40.0F;
20267 _rtB->nzcmd_i = 0.0F;
20268 _rtB->nycmd_a = 0.0F;
20269 _rtB->phiLcmd_c = 0.0F;
20270 _rtB->thetaLcmd_d = 0.0F;
20271 _rtB->psiLcmd_i = 0.0F;
20272 _rtB->gamacmd_k = 0.0F;
20273 _rtB->hcmd_j = _rtB->HrefAL;
20274 _rtB->yrwcmd_o = 0.0F;
20275 _rtB->apfix_p = 0;
20276 _rtB->apcheck_p = 0;
20277 _rtB->fmLandtoTaxi = 0;
20278 _rtB->K_k = _rtB->Sum_mn;
20279 _rtB->StartEmNav_a = 0;
20280 ((int32_T *)ssGetDWork(S, 308))[0] = 0;
20281
20282 /* Entry Internal 'Landing': '<S326>:38' */
20283 /* Transition: '<S326>:347' */
20284 ((uint8_T *)ssGetDWork(S, 534))[0] = AFCS_MODEL1_IN_Pre_Land;
20285
20286 /* Entry 'Pre_Land': '<S326>:357' */
20287 ((int32_T *)ssGetDWork(S, 301))[0] = 0;
20288
20289 /* Entry Internal 'Pre_Land': '<S326>:357' */
20290 ((uint8_T *)ssGetDWork(S, 535))[0] = 1U;
20291 ((uint8_T *)ssGetDWork(S, 537))[0] = 1U;
20292
20293 /* Entry Internal 'XYPositionControl': '<S326>:367' */
20294 /* Transition: '<S326>:368' */
20295 ((uint8_T *)ssGetDWork(S, 536))[0] = AFCS_MODEL1_IN_YrwControl1;
20296
20297 /* Entry Internal 'YrwControl1': '<S326>:369' */
20298 /* Transition: '<S326>:370' */
20299 ((uint8_T *)ssGetDWork(S, 538))[0] = AFCS_MODEL1_IN_phiLcontrol;
20300
20301 /* Entry 'phiLcontrol': '<S326>:372' */
20302 _rtB->aplatL_l = 5;
20303 _rtB->psdcmd_i = 0.0F;
20304 ((int32_T *)ssGetDWork(S, 305))[0] = 0;
20305 } else {
20306 AFCS_MODEL1_Landing(S);
20307 }
20308
20309 /* End of Chart: '<S316>/Landing2' */
20310 }
20311
20312 if (1) {
20313 /* SignalConversion: '<S316>/Signal Conversion' */
20314 _rtB->configure = _rtB->configure_gv;
20315
20316 /* SignalConversion: '<S316>/Signal Conversion' */
20317 _rtB->vtcmd = _rtB->vtcmd_f;
20318
20319 /* SignalConversion: '<S316>/Signal Conversion' */
20320 _rtB->nzcmd = _rtB->nzcmd_i;
20321
20322 /* SignalConversion: '<S316>/Signal Conversion' */
20323 _rtB->nycmd = _rtB->nycmd_a;
20324
20325 /* SignalConversion: '<S316>/Signal Conversion' */
20326 _rtB->phiLcmd = _rtB->phiLcmd_c;
20327
20328 /* SignalConversion: '<S316>/Signal Conversion' */
20329 _rtB->thetaLcmd = _rtB->thetaLcmd_d;
20330
20331 /* SignalConversion: '<S316>/Signal Conversion' */
20332 _rtB->psiLcmd = _rtB->psiLcmd_i;
20333
20334 /* SignalConversion: '<S316>/Signal Conversion' */
20335 _rtB->gamacmd = _rtB->gamacmd_k;
20336
20337 /* SignalConversion: '<S316>/Signal Conversion' */
20338 _rtB->hcmd = _rtB->hcmd_j;
20339
20340 /* SignalConversion: '<S316>/Signal Conversion' */
20341 _rtB->psdcmd = _rtB->psdcmd_i;
20342
20343 /* SignalConversion: '<S316>/Signal Conversion' */
20344 _rtB->yrwcmd = _rtB->yrwcmd_o;
20345
20346 /* SignalConversion: '<S316>/Signal Conversion' */
20347 _rtB->apthL = _rtB->apthL_e;
20348
20349 /* SignalConversion: '<S316>/Signal Conversion' */
20350 _rtB->apcheck = _rtB->apcheck_p;
20351
20352 /* SignalConversion: '<S316>/Signal Conversion' */
20353 _rtB->apfix = _rtB->apfix_p;
20354
20355 /* SignalConversion: '<S316>/Signal Conversion' */
20356 _rtB->K = _rtB->K_k;
20357
20358 /* SignalConversion: '<S316>/Signal Conversion' */
20359 _rtB->StartEmNav = _rtB->StartEmNav_a;
20360
20361 /* SignalConversion: '<S316>/Signal Conversion' */
20362 _rtB->aplonL = _rtB->aplonL_f;
20363
20364 /* SignalConversion: '<S316>/Signal Conversion' */
20365 _rtB->aplatL = _rtB->aplatL_l;
20366
20367 /* SignalConversion: '<S316>/Signal Conversion' */
20368 _rtB->apyawL = _rtB->apyawL_m;
20369
20370 /* SignalConversion: '<S316>/Signal Conversion' */
20371 _rtB->apsbL = _rtB->apsbL_l;
20372
20373 /* SignalConversion: '<S316>/Signal Conversion' */
20374 _rtB->apnwsL = _rtB->apnwsL_m;
20375
20376 /* SignalConversion: '<S316>/Signal Conversion' */
20377 _rtB->apbrkL = _rtB->apbrkL_n;
20378
20379 /* SignalConversion: '<S316>/Signal Conversion' */
20380 _rtB->thrustcmd = _rtB->thrustcmd_ho;
20381 }
20382
20383 /* End of Outputs for SubSystem: '<S31>/Landing' */
20384 break;
20385
20386 case 6:
20387 /* Outputs for IfAction SubSystem: '<S31>/Taxi' incorporates:
20388 * ActionPort: '<S322>/Action Port'
20389 */
20390 if (ssIsSampleHit(S, 1, 0)) {
20391 /* Chart: '<S322>/Taxi' incorporates:
20392 * Constant: '<S17>/Constant7'
20393 * Constant: '<S17>/Constant8'
20394 */
20395 /* Gateway: AFCS_MODEL1/GNC/logic/Taxi/Taxi */
20396 /* During: AFCS_MODEL1/GNC/logic/Taxi/Taxi */
20397 if (((uint8_T *)ssGetDWork(S, 527))[0] == 0U) {
20398 /* Entry: AFCS_MODEL1/GNC/logic/Taxi/Taxi */
20399 ((uint8_T *)ssGetDWork(S, 527))[0] = 1U;
20400
20401 /* Entry Internal: AFCS_MODEL1/GNC/logic/Taxi/Taxi */
20402 ((uint8_T *)ssGetDWork(S, 528))[0] = AFCS_MODEL1_IN_Taxi;
20403
20404 /* Entry 'Taxi': '<S330>:38' */
20405 _rtB->configure_g = 1;
20406 _rtB->apthL_i = 1;
20407 _rtB->aplonL_o = 2;
20408 _rtB->aplatL_f = 2;
20409 _rtB->apyawL_f = 3;
20410 _rtB->apsbL_e = 2;
20411
20412 /* 着陆滑跑,阻力舵0.5 */
20413 _rtB->apbrkL_o = 1;
20414 _rtB->thrustcmd_h = 0.0F;
20415 _rtB->vtcmd_k = 0.0F;
20416 _rtB->nzcmd_l = 0.0F;
20417 _rtB->nycmd_m = 0.0F;
20418 _rtB->phiLcmd_l = 0.0F;
20419 _rtB->thetaLcmd_f = 2.0F;
20420 _rtB->psiLcmd_c = 0.0F;
20421 _rtB->gamacmd_p = 0.0F;
20422 _rtB->hcmd_k = 0.0F;
20423 _rtB->psdcmd_c = 0.0F;
20424 _rtB->yrwcmd_k = 0.0F;
20425 _rtB->apfix_c = 0;
20426 _rtB->apcheck_f = 0;
20427 _rtB->K_c = _rtB->UnitDelay;
20428 _rtB->StartEmNav_b = 0;
20429
20430 /* Entry Internal 'Taxi': '<S330>:38' */
20431 ((uint8_T *)ssGetDWork(S, 530))[0] = 1U;
20432
20433 /* Entry Internal 'TaxiControl': '<S330>:190' */
20434 /* Transition: '<S330>:198' */
20435 ((uint8_T *)ssGetDWork(S, 529))[0] = AFCS_MODEL1_IN_DrControl;
20436
20437 /* Entry 'DrControl': '<S330>:194' */
20438 _rtB->apnwsL_k = 1;
20439 ((int32_T *)ssGetDWork(S, 297))[0] = 0;
20440 ((uint8_T *)ssGetDWork(S, 531))[0] = 1U;
20441
20442 /* Entry Internal 'PointControl': '<S330>:199' */
20443 /* Transition: '<S330>:202' */
20444 if (_rtB->AbortTakeoffFg == 1) {
20445 /* Transition: '<S330>:204' */
20446 _rtB->K_c = 1;
20447 } else {
20448 /* Transition: '<S330>:206' */
20449 i_1 = _rtB->IndexAL + 1;
20450 if (i_1 > 127) {
20451 i_1 = 127;
20452 }
20453
20454 _rtB->K_c = (int8_T)i_1;
20455 }
20456 } else {
20457 /* During 'Taxi': '<S330>:38' */
20458 /* During 'TaxiControl': '<S330>:190' */
20459 switch (((uint8_T *)ssGetDWork(S, 529))[0]) {
20460 case AFCS_MODEL1_IN_DrControl:
20461 /* During 'DrControl': '<S330>:194' */
20462 if (((int32_T *)ssGetDWork(S, 297))[0] == 5) {
20463 /* Transition: '<S330>:191' */
20464 ((uint8_T *)ssGetDWork(S, 529))[0] = AFCS_MODEL1_IN_NwsControl;
20465
20466 /* Entry 'NwsControl': '<S330>:196' */
20467 _rtB->apnwsL_k = 2;
20468 _rtB->apbrkL_o = 2;
20469 ((int32_T *)ssGetDWork(S, 299))[0] = 0;
20470 } else {
20471 ((int32_T *)ssGetDWork(S, 297))[0] = AFCS_MODEL1_addup_od
20472 ((_rtB->Switch7_n == 1) && (_rtB->Switch6_j == 1), 5, ((int32_T *)
20473 ssGetDWork(S, 297))[0]);
20474 }
20475 break;
20476
20477 case AFCS_MODEL1_IN_EndControl:
20478 /* During 'EndControl': '<S330>:195' */
20479 break;
20480
20481 case AFCS_MODEL1_IN_NoDrControl:
20482 /* During 'NoDrControl': '<S330>:197' */
20483 if (((int32_T *)ssGetDWork(S, 298))[0] == 5) {
20484 /* Transition: '<S330>:193' */
20485 ((uint8_T *)ssGetDWork(S, 529))[0] = AFCS_MODEL1_IN_EndControl;
20486
20487 /* Entry 'EndControl': '<S330>:195' */
20488 _rtB->apthL_i = 1;
20489 _rtB->aplonL_o = 1;
20490 _rtB->aplatL_f = 1;
20491 _rtB->apyawL_f = 1;
20492 _rtB->apnwsL_k = 1;
20493 _rtB->apbrkL_o = 0;
20494 _rtB->apsbL_e = 1;
20495 } else {
20496 ((int32_T *)ssGetDWork(S, 298))[0] = AFCS_MODEL1_addup_od(_rtB->vt
20497 < 0.5F, 5, ((int32_T *)ssGetDWork(S, 298))[0]);
20498 }
20499 break;
20500
20501 case AFCS_MODEL1_IN_NwsControl:
20502 /* During 'NwsControl': '<S330>:196' */
20503 if (((int32_T *)ssGetDWork(S, 299))[0] == 5) {
20504 /* Transition: '<S330>:192' */
20505 ((uint8_T *)ssGetDWork(S, 529))[0] = AFCS_MODEL1_IN_ThetaControl;
20506
20507 /* Entry 'ThetaControl': '<S330>:209' */
20508 _rtB->aplonL_o = 2;
20509 _rtB->thetaLcmd_f = -5.0F;
20510 ((int32_T *)ssGetDWork(S, 300))[0] = 0;
20511 } else {
20512 ((int32_T *)ssGetDWork(S, 299))[0] = AFCS_MODEL1_addup_od
20513 (_rtB->Vc_p < 15.0F, 5, ((int32_T *)ssGetDWork(S, 299))[0]);
20514 }
20515 break;
20516
20517 default:
20518 /* During 'ThetaControl': '<S330>:209' */
20519 if (((int32_T *)ssGetDWork(S, 300))[0] == 5) {
20520 /* Transition: '<S330>:211' */
20521 ((uint8_T *)ssGetDWork(S, 529))[0] = AFCS_MODEL1_IN_NoDrControl;
20522
20523 /* Entry 'NoDrControl': '<S330>:197' */
20524 _rtB->apyawL_f = 1;
20525 ((int32_T *)ssGetDWork(S, 298))[0] = 0;
20526 } else {
20527 ((int32_T *)ssGetDWork(S, 300))[0] = AFCS_MODEL1_addup_od
20528 (_rtB->Vc_p < 5.0F, 5, ((int32_T *)ssGetDWork(S, 300))[0]);
20529 }
20530 break;
20531 }
20532
20533 /* During 'PointControl': '<S330>:199' */
20534 }
20535
20536 /* End of Chart: '<S322>/Taxi' */
20537 }
20538
20539 if (1) {
20540 /* SignalConversion: '<S322>/Signal Conversion' */
20541 _rtB->configure = _rtB->configure_g;
20542
20543 /* SignalConversion: '<S322>/Signal Conversion' */
20544 _rtB->vtcmd = _rtB->vtcmd_k;
20545
20546 /* SignalConversion: '<S322>/Signal Conversion' */
20547 _rtB->nzcmd = _rtB->nzcmd_l;
20548
20549 /* SignalConversion: '<S322>/Signal Conversion' */
20550 _rtB->nycmd = _rtB->nycmd_m;
20551
20552 /* SignalConversion: '<S322>/Signal Conversion' */
20553 _rtB->phiLcmd = _rtB->phiLcmd_l;
20554
20555 /* SignalConversion: '<S322>/Signal Conversion' */
20556 _rtB->thetaLcmd = _rtB->thetaLcmd_f;
20557
20558 /* SignalConversion: '<S322>/Signal Conversion' */
20559 _rtB->psiLcmd = _rtB->psiLcmd_c;
20560
20561 /* SignalConversion: '<S322>/Signal Conversion' */
20562 _rtB->gamacmd = _rtB->gamacmd_p;
20563
20564 /* SignalConversion: '<S322>/Signal Conversion' */
20565 _rtB->hcmd = _rtB->hcmd_k;
20566
20567 /* SignalConversion: '<S322>/Signal Conversion' */
20568 _rtB->psdcmd = _rtB->psdcmd_c;
20569
20570 /* SignalConversion: '<S322>/Signal Conversion' */
20571 _rtB->yrwcmd = _rtB->yrwcmd_k;
20572
20573 /* SignalConversion: '<S322>/Signal Conversion' */
20574 _rtB->apthL = _rtB->apthL_i;
20575
20576 /* SignalConversion: '<S322>/Signal Conversion' */
20577 _rtB->apcheck = _rtB->apcheck_f;
20578
20579 /* SignalConversion: '<S322>/Signal Conversion' */
20580 _rtB->apfix = _rtB->apfix_c;
20581
20582 /* SignalConversion: '<S322>/Signal Conversion' */
20583 _rtB->K = _rtB->K_c;
20584
20585 /* SignalConversion: '<S322>/Signal Conversion' */
20586 _rtB->StartEmNav = _rtB->StartEmNav_b;
20587
20588 /* SignalConversion: '<S322>/Signal Conversion' */
20589 _rtB->aplonL = _rtB->aplonL_o;
20590
20591 /* SignalConversion: '<S322>/Signal Conversion' */
20592 _rtB->aplatL = _rtB->aplatL_f;
20593
20594 /* SignalConversion: '<S322>/Signal Conversion' */
20595 _rtB->apyawL = _rtB->apyawL_f;
20596
20597 /* SignalConversion: '<S322>/Signal Conversion' */
20598 _rtB->apsbL = _rtB->apsbL_e;
20599
20600 /* SignalConversion: '<S322>/Signal Conversion' */
20601 _rtB->apnwsL = _rtB->apnwsL_k;
20602
20603 /* SignalConversion: '<S322>/Signal Conversion' */
20604 _rtB->apbrkL = _rtB->apbrkL_o;
20605
20606 /* SignalConversion: '<S322>/Signal Conversion' */
20607 _rtB->thrustcmd = _rtB->thrustcmd_h;
20608 }
20609
20610 /* End of Outputs for SubSystem: '<S31>/Taxi' */
20611 break;
20612 }
20613
20614 /* End of SwitchCase: '<S31>/Switch Case' */
20615 }
20616
20617 /* Outputs for Atomic SubSystem: '<S7>/Level' */
20618 if (ssIsSampleHit(S, 1, 0)) {
20619 /* SignalConversion: '<S22>/TmpSignal ConversionAtCodeReuseSubsystemInport3' */
20620 _rtB->TmpSignalConversionAtCodeReuseS = _rtB->Saturation1_g;
20621
20622 /* SignalConversion: '<S22>/TmpSignal ConversionAtCodeReuseSubsystemInport10' incorporates:
20623 * Constant: '<S27>/Constant1'
20624 * Constant: '<S27>/Constant3'
20625 * Constant: '<S27>/Constant5'
20626 * Constant: '<S27>/Constant6'
20627 * Constant: '<S27>/Constant7'
20628 * Constant: '<S27>/Constant8'
20629 */
20630 _rtB->TmpSignalConversionAtCodeReus_b[0] = 0.0F;
20631 _rtB->TmpSignalConversionAtCodeReus_b[1] = 0.0F;
20632 _rtB->TmpSignalConversionAtCodeReus_b[2] = 0.0F;
20633 _rtB->TmpSignalConversionAtCodeReus_b[3] = 0.0F;
20634 _rtB->TmpSignalConversionAtCodeReus_b[4] = 0.0F;
20635 _rtB->TmpSignalConversionAtCodeReus_b[5] = 0.0F;
20636 }
20637
20638 if (1) {
20639 /* SignalConversion: '<S22>/TmpSignal ConversionAtCodeReuseSubsystemInport12' */
20640 _rtB->TmpSignalConversionAtCodeReus_i[0] = _rtB->decl_qgc;
20641 _rtB->TmpSignalConversionAtCodeReus_i[1] = _rtB->dacl_qgc;
20642 _rtB->TmpSignalConversionAtCodeReus_i[2] = _rtB->drcl_qgc;
20643 _rtB->TmpSignalConversionAtCodeReus_i[3] = _rtB->nwscl_qgc;
20644 _rtB->TmpSignalConversionAtCodeReus_i[4] = _rtB->sbcl_qgc;
20645 }
20646
20647 /* Outputs for Atomic SubSystem: '<S22>/CodeReuseSubsystem' */
20648 AFCS_MODEL1_CodeReuseSubsystem(S);
20649
20650 /* End of Outputs for SubSystem: '<S22>/CodeReuseSubsystem' */
20651
20652 /* End of Outputs for SubSystem: '<S7>/Level' */
20653 if (1) {
20654 /* Outport: '<Root>/De_L' */
20655 ((real32_T *)ssGetOutputPortSignal(S, 24))[0] = _rtB->decl;
20656
20657 /* Outport: '<Root>/De_R' */
20658 ((real32_T *)ssGetOutputPortSignal(S, 25))[0] = _rtB->decl;
20659
20660 /* Gain: '<S1>/Gain' */
20661 _rtB->Gain_e = -_rtB->dacl;
20662
20663 /* Outport: '<Root>/Da_L' */
20664 ((real32_T *)ssGetOutputPortSignal(S, 26))[0] = _rtB->Gain_e;
20665
20666 /* Outport: '<Root>/Da_R' */
20667 ((real32_T *)ssGetOutputPortSignal(S, 27))[0] = _rtB->dacl;
20668
20669 /* Outport: '<Root>/Dr' */
20670 ((real32_T *)ssGetOutputPortSignal(S, 28))[0] = _rtB->drcl;
20671
20672 /* Outport: '<Root>/VIAS' */
20673 ((real32_T *)ssGetOutputPortSignal(S, 29))[0] = _rtB->Va_l;
20674
20675 /* Outport: '<Root>/VTAS' */
20676 ((real32_T *)ssGetOutputPortSignal(S, 30))[0] = _rtB->Vc_p;
20677
20678 /* Outport: '<Root>/VG' */
20679 ((real32_T *)ssGetOutputPortSignal(S, 31))[0] = _rtB->vt;
20680 }
20681
20682 if (ssIsSampleHit(S, 1, 0)) {
20683 /* Outport: '<Root>/Flyphase' */
20684 ((int8_T *)ssGetOutputPortSignal(S, 32))[0] = _rtB->fmphase;
20685 }
20686
20687 if (1) {
20688 /* Switch: '<S23>/Switch1' incorporates:
20689 * Constant: '<S23>/Constant'
20690 */
20691 if (_rtB->LockFlag > 0) {
20692 _rtB->thrust = 0.0F;
20693 } else {
20694 _rtB->thrust = _rtB->GeneratedSFunction_o1;
20695 }
20696
20697 /* End of Switch: '<S23>/Switch1' */
20698 }
20699
20700 if (ssIsSampleHit(S, 1, 0)) {
20701 }
20702
20703 if (1) {
20704 /* Switch: '<S7>/Switch' incorporates:
20705 * Constant: '<S25>/Constant1'
20706 * Constant: '<S25>/Constant10'
20707 * Constant: '<S25>/Constant2'
20708 * Constant: '<S25>/Constant3'
20709 * Constant: '<S25>/Constant4'
20710 * Constant: '<S25>/Constant5'
20711 * Constant: '<S25>/Constant6'
20712 * Constant: '<S25>/Constant7'
20713 * Constant: '<S25>/Constant8'
20714 * Constant: '<S25>/Constant9'
20715 */
20716 if (_rtB->LockFlag > 0) {
20717 _rtB->Switch_nw[0] = 1000U;
20718 _rtB->Switch_nw[1] = 1500U;
20719 _rtB->Switch_nw[2] = 1500U;
20720 _rtB->Switch_nw[3] = 1500U;
20721 _rtB->Switch_nw[4] = 1500U;
20722 _rtB->Switch_nw[5] = 1500U;
20723 _rtB->Switch_nw[6] = 1000U;
20724 _rtB->Switch_nw[7] = 1500U;
20725 _rtB->Switch_nw[8] = 1000U;
20726 _rtB->Switch_nw[9] = 1000U;
20727 } else {
20728 _rtB->Switch_nw[0] = _rtB->Saturation_h4[0];
20729 _rtB->Switch_nw[1] = _rtB->Saturation_h4[1];
20730 _rtB->Switch_nw[2] = _rtB->Saturation_h4[2];
20731 _rtB->Switch_nw[3] = _rtB->Saturation_h4[3];
20732 _rtB->Switch_nw[4] = _rtB->Saturation_h4[4];
20733 _rtB->Switch_nw[5] = _rtB->Saturation_h4[5];
20734 _rtB->Switch_nw[6] = _rtB->Saturation_h4[6];
20735 _rtB->Switch_nw[7] = _rtB->Saturation_h4[7];
20736 _rtB->Switch_nw[8] = _rtB->Saturation_h4[8];
20737 _rtB->Switch_nw[9] = _rtB->Saturation_h4[9];
20738 }
20739
20740 /* End of Switch: '<S7>/Switch' */
20741 }
20742
20743 if (ssIsSampleHit(S, 1, 0)) {
20744 }
20745
20746 if (1) {
20747 /* Switch: '<S24>/Switch1' incorporates:
20748 * Constant: '<S24>/Constant5'
20749 * Saturate: '<S24>/Saturation'
20750 */
20751 if (_rtB->LogicalOperator1_b) {
20752 _rtB->UnaryMinus = 0.0;
20753 } else {
20754 if (_rtB->vt > 200.0F) {
20755 /* Saturate: '<S24>/Saturation' */
20756 _rtB->TrigonometricFunction = 200.0F;
20757 } else if (_rtB->vt < 0.01F) {
20758 /* Saturate: '<S24>/Saturation' */
20759 _rtB->TrigonometricFunction = 0.01F;
20760 } else {
20761 /* Saturate: '<S24>/Saturation' */
20762 _rtB->TrigonometricFunction = _rtB->vt;
20763 }
20764
20765 /* Product: '<S24>/Divide' incorporates:
20766 * Saturate: '<S24>/Saturation'
20767 */
20768 _rtB->TrigonometricFunction = _rtB->mhdot / _rtB->TrigonometricFunction;
20769
20770 /* Trigonometry: '<S24>/Trigonometric Function' */
20771 if (_rtB->TrigonometricFunction > 1.0F) {
20772 DXp = 1.0F;
20773 } else if (_rtB->TrigonometricFunction < -1.0F) {
20774 DXp = -1.0F;
20775 } else {
20776 DXp = _rtB->TrigonometricFunction;
20777 }
20778
20779 _rtB->TrigonometricFunction = (real32_T)asin(DXp);
20780
20781 /* End of Trigonometry: '<S24>/Trigonometric Function' */
20782
20783 /* Gain: '<S291>/Unit Conversion' */
20784 _rtB->UnitConversion_f = 57.2957802F * _rtB->TrigonometricFunction;
20785 _rtB->UnaryMinus = _rtB->UnitConversion_f;
20786 }
20787
20788 /* End of Switch: '<S24>/Switch1' */
20789
20790 /* DataTypeConversion: '<S24>/Data Type Conversion7' */
20791 _rtB->gama = (real32_T)_rtB->UnaryMinus;
20792 }
20793
20794 if (ssIsSampleHit(S, 1, 0)) {
20795 /* Chart: '<S21>/delayblock1' */
20796 /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock1 */
20797 /* During: AFCS_MODEL1/GNC/Calibration/delayblock1 */
20798 if (((uint8_T *)ssGetDWork(S, 609))[0] == 0U) {
20799 /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock1 */
20800 ((uint8_T *)ssGetDWork(S, 609))[0] = 1U;
20801
20802 /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock1 */
20803 ((uint8_T *)ssGetDWork(S, 610))[0] = AFCS_MODEL1_IN_delayblock;
20804
20805 /* Entry 'delayblock': '<S48>:38' */
20806 /* Entry Internal 'delayblock': '<S48>:38' */
20807 /* Transition: '<S48>:164' */
20808 if (AFCS_MODEL1_rtC(S)->DataTypeConversion1 == 0) {
20809 /* Transition: '<S48>:160' */
20810 ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Input0;
20811
20812 /* Entry 'Input0': '<S48>:138' */
20813 ((int32_T *)ssGetDWork(S, 342))[0] = 0;
20814 } else {
20815 /* Transition: '<S48>:166' */
20816 ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Input1;
20817
20818 /* Entry 'Input1': '<S48>:165' */
20819 ((int32_T *)ssGetDWork(S, 342))[0] = 0;
20820 }
20821 } else {
20822 /* During 'delayblock': '<S48>:38' */
20823 switch (((uint8_T *)ssGetDWork(S, 611))[0]) {
20824 case AFCS_MODEL1_IN_Input0:
20825 /* During 'Input0': '<S48>:138' */
20826 if (((int32_T *)ssGetDWork(S, 342))[0] == 20) {
20827 /* Transition: '<S48>:137' */
20828 ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Output0;
20829
20830 /* Entry 'Output0': '<S48>:130' */
20831 ((int32_T *)ssGetDWork(S, 342))[0] = 0;
20832 } else if (AFCS_MODEL1_rtC(S)->DataTypeConversion1 == 1) {
20833 /* Transition: '<S48>:161' */
20834 ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Input1;
20835
20836 /* Entry 'Input1': '<S48>:165' */
20837 ((int32_T *)ssGetDWork(S, 342))[0] = 0;
20838 } else {
20839 ((int32_T *)ssGetDWork(S, 342))[0] = AFCS_MODEL1_addup(1, 20,
20840 ((int32_T *)ssGetDWork(S, 342))[0]);
20841 }
20842 break;
20843
20844 case AFCS_MODEL1_IN_Input1:
20845 /* During 'Input1': '<S48>:165' */
20846 if (((int32_T *)ssGetDWork(S, 342))[0] == 20) {
20847 /* Transition: '<S48>:168' */
20848 ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Output1;
20849
20850 /* Entry 'Output1': '<S48>:167' */
20851 ((int32_T *)ssGetDWork(S, 342))[0] = 0;
20852 } else if (AFCS_MODEL1_rtC(S)->DataTypeConversion1 == 0) {
20853 /* Transition: '<S48>:162' */
20854 ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Input0;
20855
20856 /* Entry 'Input0': '<S48>:138' */
20857 ((int32_T *)ssGetDWork(S, 342))[0] = 0;
20858 } else {
20859 ((int32_T *)ssGetDWork(S, 342))[0] = AFCS_MODEL1_addup(1, 20,
20860 ((int32_T *)ssGetDWork(S, 342))[0]);
20861 }
20862 break;
20863
20864 case AFCS_MODEL1_IN_Output0:
20865 /* During 'Output0': '<S48>:130' */
20866 if (AFCS_MODEL1_rtC(S)->DataTypeConversion1 == 1) {
20867 /* Transition: '<S48>:139' */
20868 ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Input1;
20869
20870 /* Entry 'Input1': '<S48>:165' */
20871 ((int32_T *)ssGetDWork(S, 342))[0] = 0;
20872 }
20873 break;
20874
20875 default:
20876 /* During 'Output1': '<S48>:167' */
20877 if (AFCS_MODEL1_rtC(S)->DataTypeConversion1 == 0) {
20878 /* Transition: '<S48>:170' */
20879 ((uint8_T *)ssGetDWork(S, 611))[0] = AFCS_MODEL1_IN_Input0;
20880
20881 /* Entry 'Input0': '<S48>:138' */
20882 ((int32_T *)ssGetDWork(S, 342))[0] = 0;
20883 }
20884 break;
20885 }
20886 }
20887
20888 /* End of Chart: '<S21>/delayblock1' */
20889
20890 /* Chart: '<S21>/delayblock6' */
20891 /* Gateway: AFCS_MODEL1/GNC/Calibration/delayblock6 */
20892 /* During: AFCS_MODEL1/GNC/Calibration/delayblock6 */
20893 if (((uint8_T *)ssGetDWork(S, 594))[0] == 0U) {
20894 /* Entry: AFCS_MODEL1/GNC/Calibration/delayblock6 */
20895 ((uint8_T *)ssGetDWork(S, 594))[0] = 1U;
20896
20897 /* Entry Internal: AFCS_MODEL1/GNC/Calibration/delayblock6 */
20898 ((uint8_T *)ssGetDWork(S, 595))[0] = AFCS_MODEL1_IN_delayblock;
20899
20900 /* Entry 'delayblock': '<S53>:38' */
20901 /* Entry Internal 'delayblock': '<S53>:38' */
20902 /* Transition: '<S53>:164' */
20903 if (AFCS_MODEL1_rtC(S)->DataTypeConversion4 == 0) {
20904 /* Transition: '<S53>:160' */
20905 ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Input0;
20906
20907 /* Entry 'Input0': '<S53>:138' */
20908 ((int32_T *)ssGetDWork(S, 337))[0] = 0;
20909 } else {
20910 /* Transition: '<S53>:166' */
20911 ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Input1;
20912
20913 /* Entry 'Input1': '<S53>:165' */
20914 ((int32_T *)ssGetDWork(S, 337))[0] = 0;
20915 }
20916 } else {
20917 /* During 'delayblock': '<S53>:38' */
20918 switch (((uint8_T *)ssGetDWork(S, 596))[0]) {
20919 case AFCS_MODEL1_IN_Input0:
20920 /* During 'Input0': '<S53>:138' */
20921 if (((int32_T *)ssGetDWork(S, 337))[0] == 20) {
20922 /* Transition: '<S53>:137' */
20923 ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Output0;
20924
20925 /* Entry 'Output0': '<S53>:130' */
20926 ((int32_T *)ssGetDWork(S, 337))[0] = 0;
20927 } else if (AFCS_MODEL1_rtC(S)->DataTypeConversion4 == 1) {
20928 /* Transition: '<S53>:161' */
20929 ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Input1;
20930
20931 /* Entry 'Input1': '<S53>:165' */
20932 ((int32_T *)ssGetDWork(S, 337))[0] = 0;
20933 } else {
20934 ((int32_T *)ssGetDWork(S, 337))[0] = AFCS_MODEL1_addup_m(1, 20,
20935 ((int32_T *)ssGetDWork(S, 337))[0]);
20936 }
20937 break;
20938
20939 case AFCS_MODEL1_IN_Input1:
20940 /* During 'Input1': '<S53>:165' */
20941 if (((int32_T *)ssGetDWork(S, 337))[0] == 20) {
20942 /* Transition: '<S53>:168' */
20943 ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Output1;
20944
20945 /* Entry 'Output1': '<S53>:167' */
20946 ((int32_T *)ssGetDWork(S, 337))[0] = 0;
20947 } else if (AFCS_MODEL1_rtC(S)->DataTypeConversion4 == 0) {
20948 /* Transition: '<S53>:162' */
20949 ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Input0;
20950
20951 /* Entry 'Input0': '<S53>:138' */
20952 ((int32_T *)ssGetDWork(S, 337))[0] = 0;
20953 } else {
20954 ((int32_T *)ssGetDWork(S, 337))[0] = AFCS_MODEL1_addup_m(1, 20,
20955 ((int32_T *)ssGetDWork(S, 337))[0]);
20956 }
20957 break;
20958
20959 case AFCS_MODEL1_IN_Output0:
20960 /* During 'Output0': '<S53>:130' */
20961 if (AFCS_MODEL1_rtC(S)->DataTypeConversion4 == 1) {
20962 /* Transition: '<S53>:139' */
20963 ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Input1;
20964
20965 /* Entry 'Input1': '<S53>:165' */
20966 ((int32_T *)ssGetDWork(S, 337))[0] = 0;
20967 }
20968 break;
20969
20970 default:
20971 /* During 'Output1': '<S53>:167' */
20972 if (AFCS_MODEL1_rtC(S)->DataTypeConversion4 == 0) {
20973 /* Transition: '<S53>:170' */
20974 ((uint8_T *)ssGetDWork(S, 596))[0] = AFCS_MODEL1_IN_Input0;
20975
20976 /* Entry 'Input0': '<S53>:138' */
20977 ((int32_T *)ssGetDWork(S, 337))[0] = 0;
20978 }
20979 break;
20980 }
20981 }
20982
20983 /* End of Chart: '<S21>/delayblock6' */
20984
20985 /* Chart: '<S299>/AirLineUpdateSucModel' */
20986 /* Gateway: AFCS_MODEL1/GNC/NAV/PointNAV/Subsystem/AirLineUpdateSucModel */
20987 /* During: AFCS_MODEL1/GNC/NAV/PointNAV/Subsystem/AirLineUpdateSucModel */
20988 if (((uint8_T *)ssGetDWork(S, 588))[0] == 0U) {
20989 /* Entry: AFCS_MODEL1/GNC/NAV/PointNAV/Subsystem/AirLineUpdateSucModel */
20990 ((uint8_T *)ssGetDWork(S, 588))[0] = 1U;
20991
20992 /* Entry Internal: AFCS_MODEL1/GNC/NAV/PointNAV/Subsystem/AirLineUpdateSucModel */
20993 ((uint8_T *)ssGetDWork(S, 589))[0] = AFCS_M_IN_AirLineUpdateSucModel;
20994
20995 /* Entry 'AirLineUpdateSucModel': '<S300>:38' */
20996 /* Entry Internal 'AirLineUpdateSucModel': '<S300>:38' */
20997 /* Transition: '<S300>:146' */
20998 ((uint8_T *)ssGetDWork(S, 590))[0] = AFCS_MODEL1_IN_pre;
20999
21000 /* Entry 'pre': '<S300>:144' */
21001 ((int8_T *)ssGetDWork(S, 515))[0] = 0;
21002 } else {
21003 /* During 'AirLineUpdateSucModel': '<S300>:38' */
21004 switch (((uint8_T *)ssGetDWork(S, 590))[0]) {
21005 case AFCS__IN_AirLineUpdateSucModel1:
21006 /* During 'AirLineUpdateSucModel1': '<S300>:124' */
21007 if (_rtB->airLineUpdateFlag == 0) {
21008 /* Transition: '<S300>:138' */
21009 ((uint8_T *)ssGetDWork(S, 590))[0] = AFCS_MODEL1_IN_pre;
21010
21011 /* Entry 'pre': '<S300>:144' */
21012 ((int8_T *)ssGetDWork(S, 515))[0] = 0;
21013 }
21014 break;
21015
21016 case AFCS_MODEL1_IN_Alt1:
21017 /* During 'Alt1': '<S300>:140' */
21018 if (((int8_T *)ssGetDWork(S, 515))[0] == 10) {
21019 /* Transition: '<S300>:123' */
21020 ((uint8_T *)ssGetDWork(S, 590))[0] = AFCS__IN_AirLineUpdateSucModel1;
21021
21022 /* Entry 'AirLineUpdateSucModel1': '<S300>:124' */
21023 } else {
21024 i_1 = ((int8_T *)ssGetDWork(S, 515))[0] + 1;
21025 if (i_1 > 127) {
21026 i_1 = 127;
21027 }
21028
21029 ((int8_T *)ssGetDWork(S, 515))[0] = (int8_T)i_1;
21030 }
21031 break;
21032
21033 default:
21034 /* During 'pre': '<S300>:144' */
21035 if (_rtB->airLineUpdateFlag == 1) {
21036 /* Transition: '<S300>:147' */
21037 ((uint8_T *)ssGetDWork(S, 590))[0] = AFCS_MODEL1_IN_Alt1;
21038
21039 /* Entry 'Alt1': '<S300>:140' */
21040 ((int8_T *)ssGetDWork(S, 515))[0] = 0;
21041 }
21042 break;
21043 }
21044 }
21045
21046 /* End of Chart: '<S299>/AirLineUpdateSucModel' */
21047
21048 /* RelationalOperator: '<S311>/Compare' incorporates:
21049 * Constant: '<S311>/Constant'
21050 */
21051 _rtB->Compare_p = (_rtB->fmphase >= 3);
21052
21053 /* Delay: '<S310>/Delay' */
21054 _rtB->RelationalOperator_l = ((boolean_T *)ssGetDWork(S, 502))[0];
21055
21056 /* Logic: '<S310>/Logical Operator1' */
21057 _rtB->LogicalOperator1 = _rtB->Compare_p ^ _rtB->RelationalOperator_l;
21058
21059 /* UnitDelay: '<S309>/Unit Delay6' */
21060 _rtB->UnitDelay6 = ((int8_T *)ssGetDWork(S, 365))[0];
21061
21062 /* Chart: '<S29>/LLACalibration2' */
21063 /* Gateway: AFCS_MODEL1/GNC/Subsystem1/LLACalibration2 */
21064 /* During: AFCS_MODEL1/GNC/Subsystem1/LLACalibration2 */
21065 if (((uint8_T *)ssGetDWork(S, 583))[0] == 0U) {
21066 /* Entry: AFCS_MODEL1/GNC/Subsystem1/LLACalibration2 */
21067 ((uint8_T *)ssGetDWork(S, 583))[0] = 1U;
21068
21069 /* Entry Internal: AFCS_MODEL1/GNC/Subsystem1/LLACalibration2 */
21070 /* Transition: '<S308>:146' */
21071 ((uint8_T *)ssGetDWork(S, 584))[0] = AFCS_MODEL1_IN_AltCalibration;
21072
21073 /* Entry 'AltCalibration': '<S308>:38' */
21074 ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_pre;
21075
21076 /* Entry 'pre': '<S308>:144' */
21077 ((int32_T *)ssGetDWork(S, 331))[0] = 0;
21078 } else {
21079 /* During 'AltCalibration': '<S308>:38' */
21080 switch (((uint8_T *)ssGetDWork(S, 585))[0]) {
21081 case AFCS_MODEL1_IN_Alt1_o:
21082 /* During 'Alt1': '<S308>:140' */
21083 if (((int32_T *)ssGetDWork(S, 331))[0] == 10) {
21084 /* Transition: '<S308>:123' */
21085 ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_pre;
21086
21087 /* Entry 'pre': '<S308>:144' */
21088 ((int32_T *)ssGetDWork(S, 331))[0] = 0;
21089 } else {
21090 tmp_1 = ((int32_T *)ssGetDWork(S, 331))[0] + 1LL;
21091 if (tmp_1 > 2147483647LL) {
21092 tmp_1 = 2147483647LL;
21093 } else {
21094 if (tmp_1 < -2147483648LL) {
21095 tmp_1 = -2147483648LL;
21096 }
21097 }
21098
21099 ((int32_T *)ssGetDWork(S, 331))[0] = (int32_T)tmp_1;
21100 }
21101 break;
21102
21103 case AFCS_MODEL1_IN_Alt2:
21104 /* During 'Alt2': '<S308>:155' */
21105 if (((int32_T *)ssGetDWork(S, 331))[0] == 10) {
21106 /* Transition: '<S308>:157' */
21107 ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_pre;
21108
21109 /* Entry 'pre': '<S308>:144' */
21110 ((int32_T *)ssGetDWork(S, 331))[0] = 0;
21111 } else {
21112 tmp_1 = ((int32_T *)ssGetDWork(S, 331))[0] + 1LL;
21113 if (tmp_1 > 2147483647LL) {
21114 tmp_1 = 2147483647LL;
21115 } else {
21116 if (tmp_1 < -2147483648LL) {
21117 tmp_1 = -2147483648LL;
21118 }
21119 }
21120
21121 ((int32_T *)ssGetDWork(S, 331))[0] = (int32_T)tmp_1;
21122 }
21123 break;
21124
21125 default:
21126 /* During 'pre': '<S308>:144' */
21127 if (_rtB->LogicalOperator1) {
21128 /* Transition: '<S308>:147' */
21129 ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_Alt1_o;
21130
21131 /* Entry 'Alt1': '<S308>:140' */
21132 ((int32_T *)ssGetDWork(S, 331))[0] = 0;
21133 } else {
21134 if (_rtB->UnitDelay6 == 1) {
21135 /* Transition: '<S308>:156' */
21136 ((uint8_T *)ssGetDWork(S, 585))[0] = AFCS_MODEL1_IN_Alt2;
21137
21138 /* Entry 'Alt2': '<S308>:155' */
21139 ((int32_T *)ssGetDWork(S, 331))[0] = 0;
21140 }
21141 }
21142 break;
21143 }
21144 }
21145
21146 /* End of Chart: '<S29>/LLACalibration2' */
21147
21148 /* UnitDelay: '<S309>/Unit Delay5' */
21149 _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 366))[0];
21150
21151 /* RelationalOperator: '<S309>/Relational Operator' incorporates:
21152 * Constant: '<S7>/Constant'
21153 */
21154 _rtB->RelationalOperator_l = (_rtB->UnitDelay5_b != 0);
21155
21156 /* DataTypeConversion: '<S309>/Data Type Conversion2' */
21157 _rtB->DataTypeConversion2_l = (int8_T)_rtB->RelationalOperator_l;
21158 }
21159
21160 if (1) {
21161 /* Gain: '<S9>/Gain' */
21162 _rtB->Gain = 57.3 * _rtB->psic;
21163 }
21164
21165 if (ssIsSampleHit(S, 1, 0)) {
21166 }
21167
21168 if (1) {
21169 /* Gain: '<S337>/Unit Conversion' */
21170 _rtB->UnaryMinus = 0.017453292519943295 * _rtB->LLA0[1];
21171
21172 /* Gain: '<S332>/Gain' */
21173 _rtB->Switch_dn = -_rtB->LLA0[0];
21174
21175 /* Sum: '<S332>/Sum' incorporates:
21176 * Constant: '<S332>/Constant1'
21177 */
21178 _rtB->Switch_dn += -90.0;
21179
21180 /* Gain: '<S338>/Unit Conversion' */
21181 _rtB->Switch_dn *= 0.017453292519943295;
21182
21183 /* SignalConversion: '<S341>/TmpSignal ConversionAtsincosInport1' incorporates:
21184 * Constant: '<S332>/Constant4'
21185 */
21186 _rtB->Sum1_m[0] = _rtB->UnaryMinus;
21187 _rtB->Sum1_m[1] = _rtB->Switch_dn;
21188 _rtB->Sum1_m[2] = 0.0;
21189
21190 /* Trigonometry: '<S341>/sincos' */
21191 TrigonometricFunction4_o = cos(_rtB->Sum1_m[0]);
21192 _rtB->Sum1_m[0] = sin(_rtB->Sum1_m[0]);
21193 _rtB->Sum_o[0] = TrigonometricFunction4_o;
21194 TrigonometricFunction4_o = cos(_rtB->Sum1_m[1]);
21195 _rtB->Sum1_m[1] = sin(_rtB->Sum1_m[1]);
21196 _rtB->Sum_o[1] = TrigonometricFunction4_o;
21197 TrigonometricFunction4_o = cos(_rtB->Sum1_m[2]);
21198 _rtB->Sum1_m[2] = sin(_rtB->Sum1_m[2]);
21199 _rtB->Sum_o[2] = TrigonometricFunction4_o;
21200
21201 /* Fcn: '<S341>/Fcn11' */
21202 _rtB->VectorConcatenate_m[0] = _rtB->Sum_o[1] * _rtB->Sum_o[0];
21203
21204 /* Fcn: '<S341>/Fcn21' */
21205 _rtB->VectorConcatenate_m[1] = _rtB->Sum1_m[2] * _rtB->Sum1_m[1] *
21206 _rtB->Sum_o[0] - _rtB->Sum_o[2] * _rtB->Sum1_m[0];
21207
21208 /* Fcn: '<S341>/Fcn31' */
21209 _rtB->VectorConcatenate_m[2] = _rtB->Sum_o[2] * _rtB->Sum1_m[1] *
21210 _rtB->Sum_o[0] + _rtB->Sum1_m[2] * _rtB->Sum1_m[0];
21211
21212 /* Fcn: '<S341>/Fcn12' */
21213 _rtB->VectorConcatenate_m[3] = _rtB->Sum_o[1] * _rtB->Sum1_m[0];
21214
21215 /* Fcn: '<S341>/Fcn22' */
21216 _rtB->VectorConcatenate_m[4] = _rtB->Sum1_m[2] * _rtB->Sum1_m[1] *
21217 _rtB->Sum1_m[0] + _rtB->Sum_o[2] * _rtB->Sum_o[0];
21218
21219 /* Fcn: '<S341>/Fcn32' */
21220 _rtB->VectorConcatenate_m[5] = _rtB->Sum_o[2] * _rtB->Sum1_m[1] *
21221 _rtB->Sum1_m[0] - _rtB->Sum1_m[2] * _rtB->Sum_o[0];
21222
21223 /* Fcn: '<S341>/Fcn13' */
21224 _rtB->VectorConcatenate_m[6] = -_rtB->Sum1_m[1];
21225
21226 /* Fcn: '<S341>/Fcn23' */
21227 _rtB->VectorConcatenate_m[7] = _rtB->Sum1_m[2] * _rtB->Sum_o[1];
21228
21229 /* Fcn: '<S341>/Fcn33' */
21230 _rtB->VectorConcatenate_m[8] = _rtB->Sum_o[2] * _rtB->Sum_o[1];
21231
21232 /* Abs: '<S418>/Abs' */
21233 _rtB->UnaryMinus = fabs(_rtB->Lat);
21234
21235 /* RelationalOperator: '<S419>/Compare' incorporates:
21236 * Constant: '<S419>/Constant'
21237 */
21238 _rtB->Compare_in = (_rtB->UnaryMinus > 180.0);
21239
21240 /* Switch: '<S418>/Switch' */
21241 if (_rtB->Compare_in) {
21242 /* Bias: '<S418>/Bias' */
21243 _rtB->MathFunction1_c = _rtB->Lat + 180.0;
21244
21245 /* Math: '<S418>/Math Function1' incorporates:
21246 * Constant: '<S418>/Constant2'
21247 */
21248 _rtB->MathFunction1_c = AFCS_MODEL1_rt_modd_snf(_rtB->MathFunction1_c,
21249 360.0);
21250
21251 /* Bias: '<S418>/Bias1' */
21252 _rtB->Bias1_p = _rtB->MathFunction1_c + -180.0;
21253 _rtB->Switch = _rtB->Bias1_p;
21254 } else {
21255 _rtB->Switch = _rtB->Lat;
21256 }
21257
21258 /* End of Switch: '<S418>/Switch' */
21259
21260 /* Abs: '<S415>/Abs1' */
21261 _rtB->Abs1 = fabs(_rtB->Switch);
21262
21263 /* RelationalOperator: '<S417>/Compare' incorporates:
21264 * Constant: '<S417>/Constant'
21265 */
21266 _rtB->Compare_in = (_rtB->Abs1 > 90.0);
21267
21268 /* Switch: '<S415>/Switch' */
21269 if (_rtB->Compare_in) {
21270 /* Bias: '<S415>/Bias' */
21271 _rtB->Bias1_pq = _rtB->Abs1 + -90.0;
21272
21273 /* Gain: '<S415>/Gain' */
21274 _rtB->Bias1_pq = -_rtB->Bias1_pq;
21275
21276 /* Bias: '<S415>/Bias1' */
21277 _rtB->Bias1_pq += 90.0;
21278
21279 /* Signum: '<S415>/Sign1' */
21280 if (_rtB->Switch < 0.0) {
21281 _rtB->Sign1_g = -1.0;
21282 } else if (_rtB->Switch > 0.0) {
21283 _rtB->Sign1_g = 1.0;
21284 } else if (_rtB->Switch == 0.0) {
21285 _rtB->Sign1_g = 0.0;
21286 } else {
21287 _rtB->Sign1_g = _rtB->Switch;
21288 }
21289
21290 /* End of Signum: '<S415>/Sign1' */
21291
21292 /* Product: '<S415>/Divide1' */
21293 _rtB->Divide1_o = _rtB->Sign1_g * _rtB->Bias1_pq;
21294 _rtB->UnaryMinus = _rtB->Divide1_o;
21295 } else {
21296 _rtB->UnaryMinus = _rtB->Switch;
21297 }
21298
21299 /* End of Switch: '<S415>/Switch' */
21300
21301 /* Gain: '<S409>/Unit Conversion' */
21302 _rtB->Switch_dn = 0.017453292519943295 * _rtB->UnaryMinus;
21303
21304 /* Trigonometry: '<S384>/sincos1' */
21305 _rtB->u2u3_a = sin(_rtB->Switch_dn);
21306 _rtB->Add_em = cos(_rtB->Switch_dn);
21307
21308 /* Product: '<S384>/Product3' incorporates:
21309 * Constant: '<S340>/Constant2'
21310 */
21311 _rtB->sincos_o2_j[0] = 0.0 * _rtB->u2u3_a;
21312 _rtB->sincos_o2_j[1] = 0.0 * _rtB->Add_em;
21313
21314 /* Trigonometry: '<S384>/Trigonometric Function1' */
21315 _rtB->Switch_dn = tan(_rtB->Switch_dn);
21316
21317 /* Product: '<S384>/Product2' */
21318 _rtB->Switch_dn = _rtB->Switch_dn * AFCS_MODEL1_rtC(S)->Sum *
21319 AFCS_MODEL1_rtC(S)->Sum;
21320
21321 /* Trigonometry: '<S384>/Trigonometric Function2' incorporates:
21322 * Constant: '<S384>/f'
21323 */
21324 _rtB->Switch_dn = AFCS_MODEL1_rt_atan2d_snf(_rtB->Switch_dn, 1.0);
21325
21326 /* Gain: '<S411>/Unit Conversion' */
21327 _rtB->Add_em = 57.295779513082323 * _rtB->Switch_dn;
21328
21329 /* Gain: '<S414>/Unit Conversion' */
21330 _rtB->Add_em *= 0.017453292519943295;
21331
21332 /* Trigonometry: '<S413>/Trigonometric Function' */
21333 _rtB->Add_em = sin(_rtB->Add_em);
21334
21335 /* Product: '<S413>/Product2' */
21336 _rtB->Add_em = _rtB->Add_em * _rtB->Add_em * AFCS_MODEL1_rtC(S)->Sum1_i;
21337
21338 /* Sum: '<S413>/Sum2' incorporates:
21339 * Constant: '<S413>/Constant'
21340 */
21341 _rtB->Add_em++;
21342
21343 /* Product: '<S413>/Product3' incorporates:
21344 * Constant: '<S413>/Re'
21345 */
21346 _rtB->Add_em = 4.0680631590769E+13 / _rtB->Add_em;
21347
21348 /* Sqrt: '<S413>/sqrt' */
21349 _rtB->Add_em = sqrt(_rtB->Add_em);
21350
21351 /* Trigonometry: '<S384>/sincos' */
21352 _rtB->u2u3_a = cos(_rtB->Switch_dn);
21353 _rtB->Switch_dn = sin(_rtB->Switch_dn);
21354
21355 /* Product: '<S384>/Product1' */
21356 _rtB->sincos_o1_d[0] = _rtB->Add_em * _rtB->Switch_dn;
21357 _rtB->sincos_o1_d[1] = _rtB->Add_em * _rtB->u2u3_a;
21358
21359 /* Sum: '<S384>/Sum1' */
21360 _rtB->sincos_o2_j[0] += _rtB->sincos_o1_d[0];
21361 _rtB->sincos_o2_j[1] += _rtB->sincos_o1_d[1];
21362
21363 /* Trigonometry: '<S384>/Trigonometric Function3' */
21364 _rtB->Add_em = AFCS_MODEL1_rt_atan2d_snf(_rtB->sincos_o2_j[0],
21365 _rtB->sincos_o2_j[1]);
21366
21367 /* Gain: '<S410>/Unit Conversion' */
21368 _rtB->Add_em *= 57.295779513082323;
21369
21370 /* Switch: '<S385>/Switch1' incorporates:
21371 * Constant: '<S385>/Constant'
21372 * Constant: '<S385>/Constant1'
21373 */
21374 if (_rtB->Compare_in) {
21375 _rtB->Switch_dn = 180.0;
21376 } else {
21377 _rtB->Switch_dn = 0.0;
21378 }
21379
21380 /* End of Switch: '<S385>/Switch1' */
21381
21382 /* Sum: '<S385>/Sum' */
21383 _rtB->Sum = _rtB->Switch_dn + _rtB->Lon;
21384
21385 /* Abs: '<S416>/Abs' */
21386 _rtB->Switch_dn = fabs(_rtB->Sum);
21387
21388 /* RelationalOperator: '<S420>/Compare' incorporates:
21389 * Constant: '<S420>/Constant'
21390 */
21391 _rtB->Compare_in = (_rtB->Switch_dn > 180.0);
21392
21393 /* Switch: '<S416>/Switch' */
21394 if (_rtB->Compare_in) {
21395 /* Bias: '<S416>/Bias' */
21396 _rtB->MathFunction1_e = _rtB->Sum + 180.0;
21397
21398 /* Math: '<S416>/Math Function1' incorporates:
21399 * Constant: '<S416>/Constant2'
21400 */
21401 _rtB->MathFunction1_e = AFCS_MODEL1_rt_modd_snf(_rtB->MathFunction1_e,
21402 360.0);
21403
21404 /* Bias: '<S416>/Bias1' */
21405 _rtB->Bias1_a = _rtB->MathFunction1_e + -180.0;
21406 _rtB->Switch_dn = _rtB->Bias1_a;
21407 } else {
21408 _rtB->Switch_dn = _rtB->Sum;
21409 }
21410
21411 /* End of Switch: '<S416>/Switch' */
21412
21413 /* Gain: '<S407>/Unit Conversion' */
21414 _rtB->sincos_o1_d[0] = 0.017453292519943295 * _rtB->Add_em;
21415 _rtB->sincos_o1_d[1] = 0.017453292519943295 * _rtB->Switch_dn;
21416
21417 /* Trigonometry: '<S383>/sincos' */
21418 TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[0]);
21419 _rtB->sincos_o1_d[0] = sin(_rtB->sincos_o1_d[0]);
21420 _rtB->sincos_o2_j[0] = TrigonometricFunction4_o;
21421 TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[1]);
21422 _rtB->sincos_o1_d[1] = sin(_rtB->sincos_o1_d[1]);
21423 _rtB->sincos_o2_j[1] = TrigonometricFunction4_o;
21424
21425 /* Product: '<S398>/u(1)*u(4)' */
21426 _rtB->u2u3_a = _rtB->sincos_o1_d[0] * _rtB->sincos_o2_j[1];
21427
21428 /* UnaryMinus: '<S398>/Unary Minus' */
21429 _rtB->VectorConcatenate_e[0] = -_rtB->u2u3_a;
21430
21431 /* UnaryMinus: '<S401>/Unary Minus' */
21432 _rtB->VectorConcatenate_e[1] = -_rtB->sincos_o1_d[1];
21433
21434 /* Product: '<S404>/u(3)*u(4)' */
21435 _rtB->u2u3_a = _rtB->sincos_o2_j[0] * _rtB->sincos_o2_j[1];
21436
21437 /* UnaryMinus: '<S404>/Unary Minus' */
21438 _rtB->VectorConcatenate_e[2] = -_rtB->u2u3_a;
21439
21440 /* Product: '<S399>/u(1)*u(2)' */
21441 _rtB->u2u3_a = _rtB->sincos_o1_d[0] * _rtB->sincos_o1_d[1];
21442
21443 /* UnaryMinus: '<S399>/Unary Minus' */
21444 _rtB->VectorConcatenate_e[3] = -_rtB->u2u3_a;
21445
21446 /* SignalConversion: '<S408>/ConcatBufferAtVector ConcatenateIn5' */
21447 _rtB->VectorConcatenate_e[4] = _rtB->sincos_o2_j[1];
21448
21449 /* Product: '<S405>/u(2)*u(3)' */
21450 _rtB->u2u3_a = _rtB->sincos_o1_d[1] * _rtB->sincos_o2_j[0];
21451
21452 /* UnaryMinus: '<S405>/Unary Minus' */
21453 _rtB->VectorConcatenate_e[5] = -_rtB->u2u3_a;
21454
21455 /* SignalConversion: '<S408>/ConcatBufferAtVector ConcatenateIn7' */
21456 _rtB->VectorConcatenate_e[6] = _rtB->sincos_o2_j[0];
21457
21458 /* SignalConversion: '<S408>/ConcatBufferAtVector ConcatenateIn8' incorporates:
21459 * Constant: '<S403>/Constant'
21460 */
21461 _rtB->VectorConcatenate_e[7] = 0.0;
21462
21463 /* UnaryMinus: '<S406>/Unary Minus' */
21464 _rtB->VectorConcatenate_e[8] = -_rtB->sincos_o1_d[0];
21465
21466 /* Math: '<S340>/Math Function1' */
21467 for (i_1 = 0; i_1 < 3; i_1++) {
21468 _rtB->MathFunction_kc[3 * i_1] = _rtB->VectorConcatenate_e[i_1];
21469 _rtB->MathFunction_kc[1 + 3 * i_1] = _rtB->VectorConcatenate_e[i_1 + 3];
21470 _rtB->MathFunction_kc[2 + 3 * i_1] = _rtB->VectorConcatenate_e[i_1 + 6];
21471 }
21472
21473 /* End of Math: '<S340>/Math Function1' */
21474
21475 /* Gain: '<S421>/Unit Conversion' */
21476 _rtB->Add_em *= 0.017453292519943295;
21477
21478 /* Trigonometry: '<S386>/Trigonometric Function' */
21479 _rtB->Add_em = sin(_rtB->Add_em);
21480
21481 /* Product: '<S386>/Product2' */
21482 _rtB->Add_em = _rtB->Add_em * _rtB->Add_em * AFCS_MODEL1_rtC(S)->Sum1_a;
21483
21484 /* Sum: '<S386>/Sum2' incorporates:
21485 * Constant: '<S386>/Constant'
21486 */
21487 _rtB->Add_em++;
21488
21489 /* Product: '<S386>/Product3' incorporates:
21490 * Constant: '<S386>/Re'
21491 */
21492 _rtB->Add_em = 4.0680631590769E+13 / _rtB->Add_em;
21493
21494 /* Sqrt: '<S386>/sqrt' */
21495 _rtB->Add_em = sqrt(_rtB->Add_em);
21496
21497 /* UnaryMinus: '<S340>/Unary Minus' */
21498 _rtB->Add_em = -_rtB->Add_em;
21499
21500 /* SignalConversion: '<S340>/TmpSignal ConversionAtProduct1Inport2' incorporates:
21501 * Constant: '<S340>/Constant'
21502 */
21503 _rtB->Sum_o[0] = 0.0;
21504 _rtB->Sum_o[1] = 0.0;
21505 _rtB->Sum_o[2] = _rtB->Add_em;
21506
21507 /* Product: '<S340>/Product1' */
21508 for (i_1 = 0; i_1 < 3; i_1++) {
21509 _rtB->Sum1_m[i_1] = 0.0;
21510 _rtB->Sum1_m[i_1] += _rtB->MathFunction_kc[i_1] * _rtB->Sum_o[0];
21511 _rtB->Sum1_m[i_1] += _rtB->MathFunction_kc[i_1 + 3] * _rtB->Sum_o[1];
21512 _rtB->Sum1_m[i_1] += _rtB->MathFunction_kc[i_1 + 6] * _rtB->Sum_o[2];
21513 }
21514
21515 /* End of Product: '<S340>/Product1' */
21516
21517 /* Gain: '<S396>/Unit Conversion' */
21518 _rtB->sincos_o1_d[0] = 0.017453292519943295 * _rtB->UnaryMinus;
21519 _rtB->sincos_o1_d[1] = 0.017453292519943295 * _rtB->Switch_dn;
21520
21521 /* Trigonometry: '<S382>/sincos' */
21522 TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[0]);
21523 _rtB->sincos_o1_d[0] = sin(_rtB->sincos_o1_d[0]);
21524 _rtB->sincos_o2_j[0] = TrigonometricFunction4_o;
21525 TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[1]);
21526 _rtB->sincos_o1_d[1] = sin(_rtB->sincos_o1_d[1]);
21527 _rtB->sincos_o2_j[1] = TrigonometricFunction4_o;
21528
21529 /* Product: '<S387>/u(1)*u(4)' */
21530 _rtB->Add_em = _rtB->sincos_o1_d[0] * _rtB->sincos_o2_j[1];
21531
21532 /* UnaryMinus: '<S387>/Unary Minus' */
21533 _rtB->VectorConcatenate_bv[0] = -_rtB->Add_em;
21534
21535 /* UnaryMinus: '<S390>/Unary Minus' */
21536 _rtB->VectorConcatenate_bv[1] = -_rtB->sincos_o1_d[1];
21537
21538 /* Product: '<S393>/u(3)*u(4)' */
21539 _rtB->Add_em = _rtB->sincos_o2_j[0] * _rtB->sincos_o2_j[1];
21540
21541 /* UnaryMinus: '<S393>/Unary Minus' */
21542 _rtB->VectorConcatenate_bv[2] = -_rtB->Add_em;
21543
21544 /* Product: '<S388>/u(1)*u(2)' */
21545 _rtB->Add_em = _rtB->sincos_o1_d[0] * _rtB->sincos_o1_d[1];
21546
21547 /* UnaryMinus: '<S388>/Unary Minus' */
21548 _rtB->VectorConcatenate_bv[3] = -_rtB->Add_em;
21549
21550 /* SignalConversion: '<S397>/ConcatBufferAtVector ConcatenateIn5' */
21551 _rtB->VectorConcatenate_bv[4] = _rtB->sincos_o2_j[1];
21552
21553 /* Product: '<S394>/u(2)*u(3)' */
21554 _rtB->Add_em = _rtB->sincos_o1_d[1] * _rtB->sincos_o2_j[0];
21555
21556 /* UnaryMinus: '<S394>/Unary Minus' */
21557 _rtB->VectorConcatenate_bv[5] = -_rtB->Add_em;
21558
21559 /* SignalConversion: '<S397>/ConcatBufferAtVector ConcatenateIn7' */
21560 _rtB->VectorConcatenate_bv[6] = _rtB->sincos_o2_j[0];
21561
21562 /* SignalConversion: '<S397>/ConcatBufferAtVector ConcatenateIn8' incorporates:
21563 * Constant: '<S392>/Constant'
21564 */
21565 _rtB->VectorConcatenate_bv[7] = 0.0;
21566
21567 /* UnaryMinus: '<S395>/Unary Minus' */
21568 _rtB->VectorConcatenate_bv[8] = -_rtB->sincos_o1_d[0];
21569
21570 /* Math: '<S340>/Math Function' */
21571 for (i_1 = 0; i_1 < 3; i_1++) {
21572 _rtB->MathFunction_kc[3 * i_1] = _rtB->VectorConcatenate_bv[i_1];
21573 _rtB->MathFunction_kc[1 + 3 * i_1] = _rtB->VectorConcatenate_bv[i_1 + 3];
21574 _rtB->MathFunction_kc[2 + 3 * i_1] = _rtB->VectorConcatenate_bv[i_1 + 6];
21575 }
21576
21577 /* End of Math: '<S340>/Math Function' */
21578
21579 /* UnaryMinus: '<S340>/Unary Minus1' */
21580 _rtB->Add_em = -_rtB->Altitude;
21581
21582 /* SignalConversion: '<S340>/TmpSignal ConversionAtProductInport2' incorporates:
21583 * Constant: '<S340>/Constant1'
21584 */
21585 _rtB->Sum_o[0] = 0.0;
21586 _rtB->Sum_o[1] = 0.0;
21587 _rtB->Sum_o[2] = _rtB->Add_em;
21588 for (i_1 = 0; i_1 < 3; i_1++) {
21589 /* Product: '<S340>/Product' */
21590 _rtB->TmpSignalConversionAtProductI_c[i_1] = 0.0;
21591 _rtB->TmpSignalConversionAtProductI_c[i_1] += _rtB->MathFunction_kc[i_1] *
21592 _rtB->Sum_o[0];
21593 _rtB->TmpSignalConversionAtProductI_c[i_1] += _rtB->MathFunction_kc[i_1 +
21594 3] * _rtB->Sum_o[1];
21595 _rtB->TmpSignalConversionAtProductI_c[i_1] += _rtB->MathFunction_kc[i_1 +
21596 6] * _rtB->Sum_o[2];
21597
21598 /* Sum: '<S340>/Sum' */
21599 _rtB->Sum1_m[i_1] += _rtB->TmpSignalConversionAtProductI_c[i_1];
21600 }
21601
21602 /* Abs: '<S378>/Abs' */
21603 _rtB->Add_em = fabs(_rtB->LLA0[0]);
21604
21605 /* RelationalOperator: '<S379>/Compare' incorporates:
21606 * Constant: '<S379>/Constant'
21607 */
21608 _rtB->Compare_in = (_rtB->Add_em > 180.0);
21609
21610 /* Switch: '<S378>/Switch' */
21611 if (_rtB->Compare_in) {
21612 /* Bias: '<S378>/Bias' */
21613 _rtB->MathFunction1_d = _rtB->LLA0[0] + 180.0;
21614
21615 /* Math: '<S378>/Math Function1' incorporates:
21616 * Constant: '<S378>/Constant2'
21617 */
21618 _rtB->MathFunction1_d = AFCS_MODEL1_rt_modd_snf(_rtB->MathFunction1_d,
21619 360.0);
21620
21621 /* Bias: '<S378>/Bias1' */
21622 _rtB->Bias1_b = _rtB->MathFunction1_d + -180.0;
21623 _rtB->Switch_l = _rtB->Bias1_b;
21624 } else {
21625 _rtB->Switch_l = _rtB->LLA0[0];
21626 }
21627
21628 /* End of Switch: '<S378>/Switch' */
21629
21630 /* Abs: '<S375>/Abs1' */
21631 _rtB->Abs1_l = fabs(_rtB->Switch_l);
21632
21633 /* RelationalOperator: '<S377>/Compare' incorporates:
21634 * Constant: '<S377>/Constant'
21635 */
21636 _rtB->Compare_in = (_rtB->Abs1_l > 90.0);
21637
21638 /* Switch: '<S375>/Switch' */
21639 if (_rtB->Compare_in) {
21640 /* Bias: '<S375>/Bias' */
21641 _rtB->Bias1_ca = _rtB->Abs1_l + -90.0;
21642
21643 /* Gain: '<S375>/Gain' */
21644 _rtB->Bias1_ca = -_rtB->Bias1_ca;
21645
21646 /* Bias: '<S375>/Bias1' */
21647 _rtB->Bias1_ca += 90.0;
21648
21649 /* Signum: '<S375>/Sign1' */
21650 if (_rtB->Switch_l < 0.0) {
21651 _rtB->Sign1_o = -1.0;
21652 } else if (_rtB->Switch_l > 0.0) {
21653 _rtB->Sign1_o = 1.0;
21654 } else if (_rtB->Switch_l == 0.0) {
21655 _rtB->Sign1_o = 0.0;
21656 } else {
21657 _rtB->Sign1_o = _rtB->Switch_l;
21658 }
21659
21660 /* End of Signum: '<S375>/Sign1' */
21661
21662 /* Product: '<S375>/Divide1' */
21663 _rtB->Divide1_c = _rtB->Sign1_o * _rtB->Bias1_ca;
21664 _rtB->Add_em = _rtB->Divide1_c;
21665 } else {
21666 _rtB->Add_em = _rtB->Switch_l;
21667 }
21668
21669 /* End of Switch: '<S375>/Switch' */
21670
21671 /* Gain: '<S369>/Unit Conversion' */
21672 _rtB->UnaryMinus = 0.017453292519943295 * _rtB->Add_em;
21673
21674 /* Trigonometry: '<S344>/sincos1' */
21675 _rtB->Switch_dn = sin(_rtB->UnaryMinus);
21676 _rtB->u2u3_a = cos(_rtB->UnaryMinus);
21677
21678 /* Product: '<S344>/Product3' incorporates:
21679 * Constant: '<S339>/Constant2'
21680 */
21681 _rtB->sincos_o1_d[0] = 0.0 * _rtB->Switch_dn;
21682 _rtB->sincos_o1_d[1] = 0.0 * _rtB->u2u3_a;
21683
21684 /* Trigonometry: '<S344>/Trigonometric Function1' */
21685 _rtB->UnaryMinus = tan(_rtB->UnaryMinus);
21686
21687 /* Product: '<S344>/Product2' */
21688 _rtB->UnaryMinus = _rtB->UnaryMinus * AFCS_MODEL1_rtC(S)->Sum_dc *
21689 AFCS_MODEL1_rtC(S)->Sum_dc;
21690
21691 /* Trigonometry: '<S344>/Trigonometric Function2' incorporates:
21692 * Constant: '<S344>/f'
21693 */
21694 _rtB->UnaryMinus = AFCS_MODEL1_rt_atan2d_snf(_rtB->UnaryMinus, 1.0);
21695
21696 /* Gain: '<S371>/Unit Conversion' */
21697 _rtB->Switch_dn = 57.295779513082323 * _rtB->UnaryMinus;
21698
21699 /* Gain: '<S374>/Unit Conversion' */
21700 _rtB->Switch_dn *= 0.017453292519943295;
21701
21702 /* Trigonometry: '<S373>/Trigonometric Function' */
21703 _rtB->Switch_dn = sin(_rtB->Switch_dn);
21704
21705 /* Product: '<S373>/Product2' */
21706 _rtB->Switch_dn = _rtB->Switch_dn * _rtB->Switch_dn * AFCS_MODEL1_rtC(S)
21707 ->Sum1_f;
21708
21709 /* Sum: '<S373>/Sum2' incorporates:
21710 * Constant: '<S373>/Constant'
21711 */
21712 _rtB->Switch_dn++;
21713
21714 /* Product: '<S373>/Product3' incorporates:
21715 * Constant: '<S373>/Re'
21716 */
21717 _rtB->Switch_dn = 4.0680631590769E+13 / _rtB->Switch_dn;
21718
21719 /* Sqrt: '<S373>/sqrt' */
21720 _rtB->Switch_dn = sqrt(_rtB->Switch_dn);
21721
21722 /* Trigonometry: '<S344>/sincos' */
21723 _rtB->u2u3_a = cos(_rtB->UnaryMinus);
21724 _rtB->UnaryMinus = sin(_rtB->UnaryMinus);
21725
21726 /* Product: '<S344>/Product1' */
21727 _rtB->sincos_o2_j[0] = _rtB->Switch_dn * _rtB->UnaryMinus;
21728 _rtB->sincos_o2_j[1] = _rtB->Switch_dn * _rtB->u2u3_a;
21729
21730 /* Sum: '<S344>/Sum1' */
21731 _rtB->sincos_o1_d[0] += _rtB->sincos_o2_j[0];
21732 _rtB->sincos_o1_d[1] += _rtB->sincos_o2_j[1];
21733
21734 /* Trigonometry: '<S344>/Trigonometric Function3' */
21735 _rtB->UnaryMinus = AFCS_MODEL1_rt_atan2d_snf(_rtB->sincos_o1_d[0],
21736 _rtB->sincos_o1_d[1]);
21737
21738 /* Gain: '<S370>/Unit Conversion' */
21739 _rtB->UnaryMinus *= 57.295779513082323;
21740
21741 /* Switch: '<S345>/Switch1' incorporates:
21742 * Constant: '<S345>/Constant'
21743 * Constant: '<S345>/Constant1'
21744 */
21745 if (_rtB->Compare_in) {
21746 _rtB->Switch_dn = 180.0;
21747 } else {
21748 _rtB->Switch_dn = 0.0;
21749 }
21750
21751 /* End of Switch: '<S345>/Switch1' */
21752
21753 /* Sum: '<S345>/Sum' */
21754 _rtB->Sum_j = _rtB->Switch_dn + _rtB->LLA0[1];
21755
21756 /* Abs: '<S376>/Abs' */
21757 _rtB->Switch_dn = fabs(_rtB->Sum_j);
21758
21759 /* RelationalOperator: '<S380>/Compare' incorporates:
21760 * Constant: '<S380>/Constant'
21761 */
21762 _rtB->Compare_in = (_rtB->Switch_dn > 180.0);
21763
21764 /* Switch: '<S376>/Switch' */
21765 if (_rtB->Compare_in) {
21766 /* Bias: '<S376>/Bias' */
21767 _rtB->MathFunction1_f = _rtB->Sum_j + 180.0;
21768
21769 /* Math: '<S376>/Math Function1' incorporates:
21770 * Constant: '<S376>/Constant2'
21771 */
21772 _rtB->MathFunction1_f = AFCS_MODEL1_rt_modd_snf(_rtB->MathFunction1_f,
21773 360.0);
21774
21775 /* Bias: '<S376>/Bias1' */
21776 _rtB->Bias1_c = _rtB->MathFunction1_f + -180.0;
21777 _rtB->Switch_dn = _rtB->Bias1_c;
21778 } else {
21779 _rtB->Switch_dn = _rtB->Sum_j;
21780 }
21781
21782 /* End of Switch: '<S376>/Switch' */
21783
21784 /* Gain: '<S367>/Unit Conversion' */
21785 _rtB->sincos_o1_d[0] = 0.017453292519943295 * _rtB->UnaryMinus;
21786 _rtB->sincos_o1_d[1] = 0.017453292519943295 * _rtB->Switch_dn;
21787
21788 /* Trigonometry: '<S343>/sincos' */
21789 TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[0]);
21790 _rtB->sincos_o1_d[0] = sin(_rtB->sincos_o1_d[0]);
21791 _rtB->sincos_o2_j[0] = TrigonometricFunction4_o;
21792 TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[1]);
21793 _rtB->sincos_o1_d[1] = sin(_rtB->sincos_o1_d[1]);
21794 _rtB->sincos_o2_j[1] = TrigonometricFunction4_o;
21795
21796 /* Product: '<S358>/u(1)*u(4)' */
21797 _rtB->u2u3_a = _rtB->sincos_o1_d[0] * _rtB->sincos_o2_j[1];
21798
21799 /* UnaryMinus: '<S358>/Unary Minus' */
21800 _rtB->VectorConcatenate_i[0] = -_rtB->u2u3_a;
21801
21802 /* UnaryMinus: '<S361>/Unary Minus' */
21803 _rtB->VectorConcatenate_i[1] = -_rtB->sincos_o1_d[1];
21804
21805 /* Product: '<S364>/u(3)*u(4)' */
21806 _rtB->u2u3_a = _rtB->sincos_o2_j[0] * _rtB->sincos_o2_j[1];
21807
21808 /* UnaryMinus: '<S364>/Unary Minus' */
21809 _rtB->VectorConcatenate_i[2] = -_rtB->u2u3_a;
21810
21811 /* Product: '<S359>/u(1)*u(2)' */
21812 _rtB->u2u3_a = _rtB->sincos_o1_d[0] * _rtB->sincos_o1_d[1];
21813
21814 /* UnaryMinus: '<S359>/Unary Minus' */
21815 _rtB->VectorConcatenate_i[3] = -_rtB->u2u3_a;
21816
21817 /* SignalConversion: '<S368>/ConcatBufferAtVector ConcatenateIn5' */
21818 _rtB->VectorConcatenate_i[4] = _rtB->sincos_o2_j[1];
21819
21820 /* Product: '<S365>/u(2)*u(3)' */
21821 _rtB->u2u3_a = _rtB->sincos_o1_d[1] * _rtB->sincos_o2_j[0];
21822
21823 /* UnaryMinus: '<S365>/Unary Minus' */
21824 _rtB->VectorConcatenate_i[5] = -_rtB->u2u3_a;
21825
21826 /* SignalConversion: '<S368>/ConcatBufferAtVector ConcatenateIn7' */
21827 _rtB->VectorConcatenate_i[6] = _rtB->sincos_o2_j[0];
21828
21829 /* SignalConversion: '<S368>/ConcatBufferAtVector ConcatenateIn8' incorporates:
21830 * Constant: '<S363>/Constant'
21831 */
21832 _rtB->VectorConcatenate_i[7] = 0.0;
21833
21834 /* UnaryMinus: '<S366>/Unary Minus' */
21835 _rtB->VectorConcatenate_i[8] = -_rtB->sincos_o1_d[0];
21836
21837 /* Math: '<S339>/Math Function1' */
21838 for (i_1 = 0; i_1 < 3; i_1++) {
21839 _rtB->MathFunction_kc[3 * i_1] = _rtB->VectorConcatenate_i[i_1];
21840 _rtB->MathFunction_kc[1 + 3 * i_1] = _rtB->VectorConcatenate_i[i_1 + 3];
21841 _rtB->MathFunction_kc[2 + 3 * i_1] = _rtB->VectorConcatenate_i[i_1 + 6];
21842 }
21843
21844 /* End of Math: '<S339>/Math Function1' */
21845
21846 /* Gain: '<S381>/Unit Conversion' */
21847 _rtB->UnaryMinus *= 0.017453292519943295;
21848
21849 /* Trigonometry: '<S346>/Trigonometric Function' */
21850 _rtB->UnaryMinus = sin(_rtB->UnaryMinus);
21851
21852 /* Product: '<S346>/Product2' */
21853 _rtB->UnaryMinus = _rtB->UnaryMinus * _rtB->UnaryMinus * AFCS_MODEL1_rtC(S
21854 )->Sum1_p;
21855
21856 /* Sum: '<S346>/Sum2' incorporates:
21857 * Constant: '<S346>/Constant'
21858 */
21859 _rtB->UnaryMinus++;
21860
21861 /* Product: '<S346>/Product3' incorporates:
21862 * Constant: '<S346>/Re'
21863 */
21864 _rtB->UnaryMinus = 4.0680631590769E+13 / _rtB->UnaryMinus;
21865
21866 /* Sqrt: '<S346>/sqrt' */
21867 _rtB->UnaryMinus = sqrt(_rtB->UnaryMinus);
21868
21869 /* UnaryMinus: '<S339>/Unary Minus' */
21870 _rtB->UnaryMinus = -_rtB->UnaryMinus;
21871
21872 /* SignalConversion: '<S339>/TmpSignal ConversionAtProduct1Inport2' incorporates:
21873 * Constant: '<S339>/Constant'
21874 */
21875 _rtB->TmpSignalConversionAtProductI_c[0] = 0.0;
21876 _rtB->TmpSignalConversionAtProductI_c[1] = 0.0;
21877 _rtB->TmpSignalConversionAtProductI_c[2] = _rtB->UnaryMinus;
21878
21879 /* Product: '<S339>/Product1' */
21880 for (i_1 = 0; i_1 < 3; i_1++) {
21881 _rtB->Sum_o[i_1] = 0.0;
21882 _rtB->Sum_o[i_1] += _rtB->MathFunction_kc[i_1] *
21883 _rtB->TmpSignalConversionAtProductI_c[0];
21884 _rtB->Sum_o[i_1] += _rtB->MathFunction_kc[i_1 + 3] *
21885 _rtB->TmpSignalConversionAtProductI_c[1];
21886 _rtB->Sum_o[i_1] += _rtB->MathFunction_kc[i_1 + 6] *
21887 _rtB->TmpSignalConversionAtProductI_c[2];
21888 }
21889
21890 /* End of Product: '<S339>/Product1' */
21891
21892 /* Gain: '<S356>/Unit Conversion' */
21893 _rtB->sincos_o1_d[0] = 0.017453292519943295 * _rtB->Add_em;
21894 _rtB->sincos_o1_d[1] = 0.017453292519943295 * _rtB->Switch_dn;
21895
21896 /* Trigonometry: '<S342>/sincos' */
21897 TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[0]);
21898 _rtB->sincos_o1_d[0] = sin(_rtB->sincos_o1_d[0]);
21899 _rtB->sincos_o2_j[0] = TrigonometricFunction4_o;
21900 TrigonometricFunction4_o = cos(_rtB->sincos_o1_d[1]);
21901 _rtB->sincos_o1_d[1] = sin(_rtB->sincos_o1_d[1]);
21902 _rtB->sincos_o2_j[1] = TrigonometricFunction4_o;
21903
21904 /* Product: '<S347>/u(1)*u(4)' */
21905 _rtB->Add_em = _rtB->sincos_o1_d[0] * _rtB->sincos_o2_j[1];
21906
21907 /* UnaryMinus: '<S347>/Unary Minus' */
21908 _rtB->VectorConcatenate_mc[0] = -_rtB->Add_em;
21909
21910 /* UnaryMinus: '<S350>/Unary Minus' */
21911 _rtB->VectorConcatenate_mc[1] = -_rtB->sincos_o1_d[1];
21912
21913 /* Product: '<S353>/u(3)*u(4)' */
21914 _rtB->Add_em = _rtB->sincos_o2_j[0] * _rtB->sincos_o2_j[1];
21915
21916 /* UnaryMinus: '<S353>/Unary Minus' */
21917 _rtB->VectorConcatenate_mc[2] = -_rtB->Add_em;
21918
21919 /* Product: '<S348>/u(1)*u(2)' */
21920 _rtB->Add_em = _rtB->sincos_o1_d[0] * _rtB->sincos_o1_d[1];
21921
21922 /* UnaryMinus: '<S348>/Unary Minus' */
21923 _rtB->VectorConcatenate_mc[3] = -_rtB->Add_em;
21924
21925 /* SignalConversion: '<S357>/ConcatBufferAtVector ConcatenateIn5' */
21926 _rtB->VectorConcatenate_mc[4] = _rtB->sincos_o2_j[1];
21927
21928 /* Product: '<S354>/u(2)*u(3)' */
21929 _rtB->Add_em = _rtB->sincos_o1_d[1] * _rtB->sincos_o2_j[0];
21930
21931 /* UnaryMinus: '<S354>/Unary Minus' */
21932 _rtB->VectorConcatenate_mc[5] = -_rtB->Add_em;
21933
21934 /* SignalConversion: '<S357>/ConcatBufferAtVector ConcatenateIn7' */
21935 _rtB->VectorConcatenate_mc[6] = _rtB->sincos_o2_j[0];
21936
21937 /* SignalConversion: '<S357>/ConcatBufferAtVector ConcatenateIn8' incorporates:
21938 * Constant: '<S352>/Constant'
21939 */
21940 _rtB->VectorConcatenate_mc[7] = 0.0;
21941
21942 /* UnaryMinus: '<S355>/Unary Minus' */
21943 _rtB->VectorConcatenate_mc[8] = -_rtB->sincos_o1_d[0];
21944
21945 /* Math: '<S339>/Math Function' */
21946 for (i_1 = 0; i_1 < 3; i_1++) {
21947 _rtB->MathFunction_kc[3 * i_1] = _rtB->VectorConcatenate_mc[i_1];
21948 _rtB->MathFunction_kc[1 + 3 * i_1] = _rtB->VectorConcatenate_mc[i_1 + 3];
21949 _rtB->MathFunction_kc[2 + 3 * i_1] = _rtB->VectorConcatenate_mc[i_1 + 6];
21950 }
21951
21952 /* End of Math: '<S339>/Math Function' */
21953
21954 /* UnaryMinus: '<S339>/Unary Minus1' */
21955 _rtB->Add_em = -_rtB->LLA0[2];
21956
21957 /* SignalConversion: '<S339>/TmpSignal ConversionAtProductInport2' incorporates:
21958 * Constant: '<S339>/Constant1'
21959 */
21960 _rtB->TmpSignalConversionAtProductI_c[0] = 0.0;
21961 _rtB->TmpSignalConversionAtProductI_c[1] = 0.0;
21962 _rtB->TmpSignalConversionAtProductI_c[2] = _rtB->Add_em;
21963 for (i_1 = 0; i_1 < 3; i_1++) {
21964 /* Product: '<S339>/Product' */
21965 _rtB->Divide1_g[i_1] = 0.0;
21966 _rtB->Divide1_g[i_1] += _rtB->MathFunction_kc[i_1] *
21967 _rtB->TmpSignalConversionAtProductI_c[0];
21968 _rtB->Divide1_g[i_1] += _rtB->MathFunction_kc[i_1 + 3] *
21969 _rtB->TmpSignalConversionAtProductI_c[1];
21970 _rtB->Divide1_g[i_1] += _rtB->MathFunction_kc[i_1 + 6] *
21971 _rtB->TmpSignalConversionAtProductI_c[2];
21972
21973 /* Sum: '<S339>/Sum' */
21974 _rtB->Sum_o[i_1] += _rtB->Divide1_g[i_1];
21975
21976 /* Sum: '<S332>/Sum1' */
21977 _rtB->Sum1_m[i_1] -= _rtB->Sum_o[i_1];
21978 }
21979
21980 /* Product: '<S332>/Divide1' */
21981 for (i_1 = 0; i_1 < 3; i_1++) {
21982 _rtB->Divide1_g[i_1] = 0.0;
21983 _rtB->Divide1_g[i_1] += _rtB->VectorConcatenate_m[i_1] * _rtB->Sum1_m[0];
21984 _rtB->Divide1_g[i_1] += _rtB->VectorConcatenate_m[i_1 + 3] * _rtB->Sum1_m
21985 [1];
21986 _rtB->Divide1_g[i_1] += _rtB->VectorConcatenate_m[i_1 + 6] * _rtB->Sum1_m
21987 [2];
21988 }
21989
21990 /* End of Product: '<S332>/Divide1' */
21991
21992 /* DataTypeConversion: '<S332>/Data Type Conversion5' */
21993 _rtB->Xnorth_jw = (real32_T)_rtB->Divide1_g[0];
21994
21995 /* DataTypeConversion: '<S332>/Data Type Conversion6' */
21996 _rtB->Yeast_i = (real32_T)_rtB->Divide1_g[1];
21997
21998 /* Sum: '<S332>/Add' */
21999 _rtB->Add_em = _rtB->Altitude - _rtB->LLA0[2];
22000
22001 /* DataTypeConversion: '<S332>/Data Type Conversion2' */
22002 _rtB->h = (real32_T)_rtB->Add_em;
22003 }
22004
22005 if (ssIsSampleHit(S, 1, 0)) {
22006 /* SignalConversion: '<S9>/TmpSignal ConversionAtTo WorkspaceInport1' */
22007 _rtB->TmpSignalConversionAtToWorks_of[0] = _rtB->Xnorth_jw;
22008 _rtB->TmpSignalConversionAtToWorks_of[1] = _rtB->Yeast_i;
22009 _rtB->TmpSignalConversionAtToWorks_of[2] = _rtB->h;
22010
22011 /* SignalConversion: '<S9>/TmpSignal ConversionAtTo Workspace1Inport1' */
22012 _rtB->TmpSignalConversionAtToWorkspac[0] = _rtB->xn;
22013 _rtB->TmpSignalConversionAtToWorkspac[1] = _rtB->ye;
22014 _rtB->TmpSignalConversionAtToWorkspac[2] = _rtB->Alt_o;
22015
22016 /* UnitDelay: '<S1>/Unit Delay' */
22017 _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 367))[0];
22018
22019 /* RelationalOperator: '<S432>/Compare' incorporates:
22020 * Constant: '<S432>/Constant'
22021 */
22022 _rtB->Compare_d = (_rtB->UnitDelay5_b >= 3);
22023
22024 /* Delay: '<S431>/Delay' */
22025 _rtB->RelationalOperator_l = ((boolean_T *)ssGetDWork(S, 503))[0];
22026
22027 /* Logic: '<S431>/Logical Operator1' */
22028 _rtB->LogicalOperator1_a = _rtB->Compare_d ^ _rtB->RelationalOperator_l;
22029
22030 /* UnitDelay: '<S430>/Unit Delay6' */
22031 _rtB->UnitDelay6_e = ((int8_T *)ssGetDWork(S, 368))[0];
22032
22033 /* Chart: '<S12>/LLACalibration2' */
22034 /* Gateway: AFCS_MODEL1/Subsystem2/LLACalibration2 */
22035 /* During: AFCS_MODEL1/Subsystem2/LLACalibration2 */
22036 if (((uint8_T *)ssGetDWork(S, 521))[0] == 0U) {
22037 /* Entry: AFCS_MODEL1/Subsystem2/LLACalibration2 */
22038 ((uint8_T *)ssGetDWork(S, 521))[0] = 1U;
22039
22040 /* Entry Internal: AFCS_MODEL1/Subsystem2/LLACalibration2 */
22041 /* Transition: '<S429>:146' */
22042 ((uint8_T *)ssGetDWork(S, 522))[0] = AFCS_MODEL1_IN_AltCalibration;
22043
22044 /* Entry 'AltCalibration': '<S429>:38' */
22045 _rtB->TrackToPtCmd = 2;
22046 ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_pre;
22047
22048 /* Entry 'pre': '<S429>:144' */
22049 ((int32_T *)ssGetDWork(S, 295))[0] = 0;
22050 _rtB->ChangePt_Fg = 0;
22051 } else {
22052 /* During 'AltCalibration': '<S429>:38' */
22053 switch (((uint8_T *)ssGetDWork(S, 523))[0]) {
22054 case AFCS_MODEL1_IN_Alt1_o:
22055 /* During 'Alt1': '<S429>:140' */
22056 if (((int32_T *)ssGetDWork(S, 295))[0] == 10) {
22057 /* Transition: '<S429>:123' */
22058 ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_pre;
22059
22060 /* Entry 'pre': '<S429>:144' */
22061 ((int32_T *)ssGetDWork(S, 295))[0] = 0;
22062 _rtB->ChangePt_Fg = 0;
22063 } else {
22064 tmp_1 = ((int32_T *)ssGetDWork(S, 295))[0] + 1LL;
22065 if (tmp_1 > 2147483647LL) {
22066 tmp_1 = 2147483647LL;
22067 } else {
22068 if (tmp_1 < -2147483648LL) {
22069 tmp_1 = -2147483648LL;
22070 }
22071 }
22072
22073 ((int32_T *)ssGetDWork(S, 295))[0] = (int32_T)tmp_1;
22074 }
22075 break;
22076
22077 case AFCS_MODEL1_IN_Alt2:
22078 /* During 'Alt2': '<S429>:155' */
22079 if (((int32_T *)ssGetDWork(S, 295))[0] == 10) {
22080 /* Transition: '<S429>:157' */
22081 ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_pre;
22082
22083 /* Entry 'pre': '<S429>:144' */
22084 ((int32_T *)ssGetDWork(S, 295))[0] = 0;
22085 _rtB->ChangePt_Fg = 0;
22086 } else {
22087 tmp_1 = ((int32_T *)ssGetDWork(S, 295))[0] + 1LL;
22088 if (tmp_1 > 2147483647LL) {
22089 tmp_1 = 2147483647LL;
22090 } else {
22091 if (tmp_1 < -2147483648LL) {
22092 tmp_1 = -2147483648LL;
22093 }
22094 }
22095
22096 ((int32_T *)ssGetDWork(S, 295))[0] = (int32_T)tmp_1;
22097 }
22098 break;
22099
22100 default:
22101 /* During 'pre': '<S429>:144' */
22102 if (_rtB->LogicalOperator1_a) {
22103 /* Transition: '<S429>:147' */
22104 ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_Alt1_o;
22105
22106 /* Entry 'Alt1': '<S429>:140' */
22107 ((int32_T *)ssGetDWork(S, 295))[0] = 0;
22108 _rtB->ChangePt_Fg = 1;
22109 _rtB->TrackToPtCmd = 2;
22110 } else {
22111 if (_rtB->UnitDelay6_e == 1) {
22112 /* Transition: '<S429>:156' */
22113 ((uint8_T *)ssGetDWork(S, 523))[0] = AFCS_MODEL1_IN_Alt2;
22114
22115 /* Entry 'Alt2': '<S429>:155' */
22116 ((int32_T *)ssGetDWork(S, 295))[0] = 0;
22117 _rtB->ChangePt_Fg = 1;
22118 _rtB->TrackToPtCmd = _rtB->TargetPiont;
22119 }
22120 }
22121 break;
22122 }
22123 }
22124
22125 /* End of Chart: '<S12>/LLACalibration2' */
22126
22127 /* UnitDelay: '<S336>/Unit Delay2' */
22128 _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 369))[0];
22129
22130 /* Sum: '<S336>/Add1' */
22131 _rtB->UnitDelay5_b = (int8_T)(_rtB->ChangePt_Fg - _rtB->UnitDelay5_b);
22132
22133 /* RelationalOperator: '<S428>/Compare' incorporates:
22134 * Constant: '<S428>/Constant'
22135 */
22136 _rtB->Compare_f = (_rtB->UnitDelay5_b >= 1);
22137
22138 /* UnitDelay: '<S430>/Unit Delay5' */
22139 _rtB->UnitDelay5_b = ((int8_T *)ssGetDWork(S, 370))[0];
22140
22141 /* RelationalOperator: '<S430>/Relational Operator' */
22142 _rtB->RelationalOperator_l = (_rtB->UnitDelay5_b != _rtB->CutPiontFG);
22143
22144 /* DataTypeConversion: '<S430>/Data Type Conversion2' */
22145 _rtB->DataTypeConversion2_d = (int8_T)_rtB->RelationalOperator_l;
22146
22147 /* Memory: '<S433>/Memory1' */
22148 _rtB->Memory1_b = ((real32_T *)ssGetDWork(S, 277))[0];
22149
22150 /* Memory: '<S433>/Memory2' */
22151 _rtB->Memory2 = ((real32_T *)ssGetDWork(S, 278))[0];
22152
22153 /* Memory: '<S433>/Memory3' */
22154 _rtB->Memory3 = ((real32_T *)ssGetDWork(S, 279))[0];
22155
22156 /* Memory: '<S433>/Memory4' */
22157 _rtB->Memory4 = ((real32_T *)ssGetDWork(S, 280))[0];
22158
22159 /* Memory: '<S433>/Memory5' */
22160 _rtB->Memory5 = ((real32_T *)ssGetDWork(S, 281))[0];
22161
22162 /* Memory: '<S433>/Memory6' */
22163 _rtB->Memory6 = ((real32_T *)ssGetDWork(S, 282))[0];
22164
22165 /* Memory: '<S433>/Memory7' */
22166 _rtB->Memory7 = ((real32_T *)ssGetDWork(S, 283))[0];
22167
22168 /* Memory: '<S433>/Memory8' */
22169 _rtB->Memory8 = ((real32_T *)ssGetDWork(S, 284))[0];
22170
22171 /* Memory: '<S433>/Memory9' */
22172 _rtB->Memory9 = ((real32_T *)ssGetDWork(S, 285))[0];
22173
22174 /* Constant: '<S17>/Constant12' */
22175 _rtB->mAHLow = 6000.0F;
22176 }
22177
22178 UNUSED_PARAMETER(tid);
22179}
22180
22181/* Update for root system: '<Root>' */
22182#define MDL_UPDATE
22183
22184static void mdlUpdate(SimStruct *S, int_T tid)
22185{
22186 B_AFCS_MODEL1_T *_rtB;
22187 _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));
22188 if (ssIsSampleHit(S, 1, 0)) {
22189 /* Update for Memory: '<S433>/Memory10' */
22190 ((real32_T *)ssGetDWork(S, 270))[0] = _rtB->Memory9;
22191
22192 /* Update for Memory: '<S438>/Memory1' */
22193 ((real32_T *)ssGetDWork(S, 271))[0] = _rtB->decl;
22194
22195 /* Update for Memory: '<S437>/Memory1' */
22196 ((real32_T *)ssGetDWork(S, 272))[0] = _rtB->dacl;
22197
22198 /* Update for Memory: '<S436>/Memory1' */
22199 ((real32_T *)ssGetDWork(S, 273))[0] = _rtB->drcl;
22200
22201 /* Update for Memory: '<S435>/Memory1' */
22202 ((real32_T *)ssGetDWork(S, 274))[0] = _rtB->Sum3_kf;
22203
22204 /* Update for Memory: '<S434>/Memory1' */
22205 ((real32_T *)ssGetDWork(S, 275))[0] = _rtB->Sum4_h;
22206
22207 /* Update for Memory: '<S439>/Memory1' */
22208 ((real32_T *)ssGetDWork(S, 276))[0] = _rtB->DataTypeConversion_e;
22209 }
22210
22211 /* Update for Atomic SubSystem: '<S1>/Subsystem5' */
22212 /* Update for Enabled SubSystem: '<S15>/Subsystem3' incorporates:
22213 * Update for EnablePort: '<S446>/Enable'
22214 */
22215 if (((boolean_T *)ssGetDWork(S, 614))[0]) {
22216 if (1) {
22217 /* Update for Integrator: '<S779>/q' */
22218 ((int_T *)ssGetDWork(S, 344))[0] = 0;
22219
22220 /* Update for Integrator: '<S778>/p1' */
22221 ((int_T *)ssGetDWork(S, 345))[0] = 0;
22222
22223 /* Update for RateLimiter: '<S465>/Rate Limiter' */
22224 ((real_T *)ssGetDWork(S, 2))[0] = _rtB->RateLimiter[0];
22225 ((real_T *)ssGetDWork(S, 2))[1] = _rtB->RateLimiter[1];
22226 ((real_T *)ssGetDWork(S, 2))[2] = _rtB->RateLimiter[2];
22227 ((real_T *)ssGetDWork(S, 3))[0] = ssGetTaskTime(S,0);
22228
22229 /* Update for Integrator: '<S780>/ub,vb,wb' */
22230 ((int_T *)ssGetDWork(S, 346))[0] = 0;
22231
22232 /* Update for Backlash: '<S446>/Backlash' */
22233 ((real_T *)ssGetDWork(S, 4))[0] = _rtB->Backlash;
22234
22235 /* Update for Integrator: '<S470>/p,q,r ' */
22236 ((int_T *)ssGetDWork(S, 347))[0] = 0;
22237 }
22238
22239 /* Update for Atomic SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */
22240 AFCS_MODEL1_LMainGearGeometryForcesandMoments_Update(S,
22241 &_rtB->LMainGearGeometryForcesandMomen);
22242
22243 /* End of Update for SubSystem: '<S466>/L Main Gear Geometry Forces and Moments' */
22244
22245 /* Update for Atomic SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */
22246 AFCS_MODEL1_RMainGearGeometryForcesandMoments_Update(S,
22247 &_rtB->RMainGearGeometryForcesandMomen);
22248
22249 /* End of Update for SubSystem: '<S466>/R Main Gear Geometry Forces and Moments' */
22250 if (ssIsSampleHit(S, 1, 0)) {
22251 /* Update for UnitDelay: '<S682>/Unit Delay' incorporates:
22252 * Constant: '<S446>/Constant2'
22253 */
22254 ((int8_T *)ssGetDWork(S, 371))[0] = 1;
22255
22256 /* Update for UnitDelay: '<S683>/Unit Delay' incorporates:
22257 * Constant: '<S446>/Constant2'
22258 */
22259 ((int8_T *)ssGetDWork(S, 372))[0] = 1;
22260
22261 /* Update for UnitDelay: '<S619>/Unit Delay1' */
22262 ((real_T *)ssGetDWork(S, 0))[0] = _rtB->Integrator_m;
22263 }
22264
22265 if (1) {
22266 /* Update for Integrator: '<S683>/Integrator' */
22267 ((int_T *)ssGetDWork(S, 348))[0] = 0;
22268
22269 /* Update for Integrator: '<S682>/Integrator' */
22270 ((int_T *)ssGetDWork(S, 349))[0] = 0;
22271 }
22272
22273 /* Update for Atomic SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */
22274 AFCS_MODEL1_NoseGearGeometryForcesandMoments_Update(S,
22275 &_rtB->NoseGearGeometryForcesandMoment);
22276
22277 /* End of Update for SubSystem: '<S466>/Nose Gear Geometry Forces and Moments' */
22278 if (1) {
22279 /* Update for Integrator: '<S778>/p' */
22280 ((int_T *)ssGetDWork(S, 350))[0] = 0;
22281 }
22282 }
22283
22284 /* End of Update for SubSystem: '<S15>/Subsystem3' */
22285 /* End of Update for SubSystem: '<S1>/Subsystem5' */
22286 if (ssIsSampleHit(S, 1, 0)) {
22287 /* Update for UnitDelay: '<S24>/Unit Delay3' */
22288 ((int8_T *)ssGetDWork(S, 352))[0] = _rtB->fmphase;
22289
22290 /* Update for UnitDelay: '<S7>/Unit Delay' */
22291 ((int8_T *)ssGetDWork(S, 353))[0] = _rtB->K;
22292
22293 /* Update for UnitDelay: '<S7>/Unit Delay3' */
22294 ((int8_T *)ssGetDWork(S, 354))[0] = _rtB->StartEmNav;
22295
22296 /* Update for Memory: '<S9>/Memory1' */
22297 ((boolean_T *)ssGetDWork(S, 612))[0] = _rtB->Compare_f;
22298
22299 /* Update for Memory: '<S9>/Memory2' */
22300 ((int8_T *)ssGetDWork(S, 504))[0] = _rtB->TrackToPtCmd;
22301
22302 /* Update for UnitDelay: '<S9>/Unit Delay' */
22303 ((int8_T *)ssGetDWork(S, 355))[0] = _rtB->UnitDelay1_l;
22304
22305 /* Update for UnitDelay: '<S1>/Unit Delay1' */
22306 ((int8_T *)ssGetDWork(S, 356))[0] = _rtB->TakeoffUL;
22307
22308 /* Update for Atomic SubSystem: '<S9>/Trajectory planning,tracking' */
22309 /* Update for UnitDelay: '<S423>/Output' */
22310 ((uint64_T *)ssGetDWork(S, 1))[0] = _rtB->FixPtSwitch;
22311
22312 /* End of Update for SubSystem: '<S9>/Trajectory planning,tracking' */
22313
22314 /* Update for UnitDelay: '<S31>/Unit Delay' */
22315 ((int8_T *)ssGetDWork(S, 357))[0] = _rtB->fmpretotakeoff;
22316
22317 /* Update for UnitDelay: '<S31>/Unit Delay1' */
22318 ((int8_T *)ssGetDWork(S, 358))[0] = _rtB->fmTOtoClimb;
22319
22320 /* Update for UnitDelay: '<S31>/Unit Delay2' */
22321 ((int8_T *)ssGetDWork(S, 359))[0] = _rtB->fmClimbtoCruise;
22322
22323 /* Update for UnitDelay: '<S31>/Unit Delay3' */
22324 ((int8_T *)ssGetDWork(S, 360))[0] = _rtB->fmCruisetoAL;
22325
22326 /* Update for UnitDelay: '<S31>/Unit Delay4' */
22327 ((int8_T *)ssGetDWork(S, 361))[0] = _rtB->fmALtoLand;
22328
22329 /* Update for UnitDelay: '<S31>/Unit Delay7' */
22330 ((int8_T *)ssGetDWork(S, 362))[0] = _rtB->fmTOtoTaxi;
22331
22332 /* Update for UnitDelay: '<S31>/Unit Delay5' */
22333 ((int8_T *)ssGetDWork(S, 363))[0] = _rtB->fmLandtoTaxi;
22334
22335 /* Update for UnitDelay: '<S32>/Unit Delay3' */
22336 ((int8_T *)ssGetDWork(S, 364))[0] = _rtB->LiftWheelFg;
22337 }
22338
22339 if (1) {
22340 /* RTW Generated Level2 S-Function Block: '<S306>/Generated S-Function15' (StickGradientDeadZone_sf) */
22341 {
22342 SimStruct *rts = ssGetSFunction(S, 167);
22343 sfcnUpdate(rts, 0);
22344 if (ssGetErrorStatus(rts) != (NULL))
22345 return;
22346 }
22347
22348 /* RTW Generated Level2 S-Function Block: '<S305>/Generated S-Function15' (StickGradientDeadZone_sf) */
22349 {
22350 SimStruct *rts = ssGetSFunction(S, 168);
22351 sfcnUpdate(rts, 0);
22352 if (ssGetErrorStatus(rts) != (NULL))
22353 return;
22354 }
22355
22356 /* RTW Generated Level2 S-Function Block: '<S304>/Generated S-Function15' (StickGradientDeadZone_sf) */
22357 {
22358 SimStruct *rts = ssGetSFunction(S, 169);
22359 sfcnUpdate(rts, 0);
22360 if (ssGetErrorStatus(rts) != (NULL))
22361 return;
22362 }
22363 }
22364
22365 /* Update for Atomic SubSystem: '<S7>/Level' */
22366
22367 /* Update for Atomic SubSystem: '<S22>/CodeReuseSubsystem' */
22368 AFCS_MODEL1_CodeReuseSubsystem_Update(S);
22369
22370 /* End of Update for SubSystem: '<S22>/CodeReuseSubsystem' */
22371
22372 /* End of Update for SubSystem: '<S7>/Level' */
22373 if (ssIsSampleHit(S, 1, 0)) {
22374 /* Update for Delay: '<S310>/Delay' */
22375 ((boolean_T *)ssGetDWork(S, 502))[0] = _rtB->Compare_p;
22376
22377 /* Update for UnitDelay: '<S309>/Unit Delay6' */
22378 ((int8_T *)ssGetDWork(S, 365))[0] = _rtB->DataTypeConversion2_l;
22379
22380 /* Update for UnitDelay: '<S309>/Unit Delay5' incorporates:
22381 * Constant: '<S7>/Constant'
22382 */
22383 ((int8_T *)ssGetDWork(S, 366))[0] = 0;
22384
22385 /* Update for UnitDelay: '<S1>/Unit Delay' */
22386 ((int8_T *)ssGetDWork(S, 367))[0] = _rtB->fmphase;
22387
22388 /* Update for Delay: '<S431>/Delay' */
22389 ((boolean_T *)ssGetDWork(S, 503))[0] = _rtB->Compare_d;
22390
22391 /* Update for UnitDelay: '<S430>/Unit Delay6' */
22392 ((int8_T *)ssGetDWork(S, 368))[0] = _rtB->DataTypeConversion2_d;
22393
22394 /* Update for UnitDelay: '<S336>/Unit Delay2' */
22395 ((int8_T *)ssGetDWork(S, 369))[0] = _rtB->ChangePt_Fg;
22396
22397 /* Update for UnitDelay: '<S430>/Unit Delay5' */
22398 ((int8_T *)ssGetDWork(S, 370))[0] = _rtB->CutPiontFG;
22399
22400 /* Update for Memory: '<S433>/Memory1' */
22401 ((real32_T *)ssGetDWork(S, 277))[0] = _rtB->thrust;
22402
22403 /* Update for Memory: '<S433>/Memory2' */
22404 ((real32_T *)ssGetDWork(S, 278))[0] = _rtB->Memory1_b;
22405
22406 /* Update for Memory: '<S433>/Memory3' */
22407 ((real32_T *)ssGetDWork(S, 279))[0] = _rtB->Memory2;
22408
22409 /* Update for Memory: '<S433>/Memory4' */
22410 ((real32_T *)ssGetDWork(S, 280))[0] = _rtB->Memory3;
22411
22412 /* Update for Memory: '<S433>/Memory5' */
22413 ((real32_T *)ssGetDWork(S, 281))[0] = _rtB->Memory4;
22414
22415 /* Update for Memory: '<S433>/Memory6' */
22416 ((real32_T *)ssGetDWork(S, 282))[0] = _rtB->Memory5;
22417
22418 /* Update for Memory: '<S433>/Memory7' */
22419 ((real32_T *)ssGetDWork(S, 283))[0] = _rtB->Memory6;
22420
22421 /* Update for Memory: '<S433>/Memory8' */
22422 ((real32_T *)ssGetDWork(S, 284))[0] = _rtB->Memory7;
22423
22424 /* Update for Memory: '<S433>/Memory9' */
22425 ((real32_T *)ssGetDWork(S, 285))[0] = _rtB->Memory8;
22426 }
22427
22428 UNUSED_PARAMETER(tid);
22429}
22430
22431/* Derivatives for root system: '<Root>' */
22432#define MDL_DERIVATIVES
22433
22434static void mdlDerivatives(SimStruct *S)
22435{
22436 boolean_T lsat;
22437 boolean_T usat;
22438 B_AFCS_MODEL1_T *_rtB;
22439 X_AFCS_MODEL1_T *_rtX;
22440 XDot_AFCS_MODEL1_T *_rtXdot;
22441 _rtXdot = ((XDot_AFCS_MODEL1_T *) ssGetdX(S));
22442 _rtX = ((X_AFCS_MODEL1_T *) ssGetContStates(S));
22443 _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));
22444
22445 /* Derivatives for Atomic SubSystem: '<S1>/Subsystem5' */
22446 /* Derivatives for Enabled SubSystem: '<S15>/Subsystem3' */
22447 if (((boolean_T *)ssGetDWork(S, 614))[0]) {
22448 /* Derivatives for Integrator: '<S472>/Servo rate' */
22449 _rtXdot->Servorate_CSTATE = _rtB->Sum2;
22450
22451 /* Derivatives for Integrator: '<S472>/Limited Deflection' */
22452 lsat = (_rtX->LimitedDeflection_CSTATE <= -30.0);
22453 usat = (_rtX->LimitedDeflection_CSTATE >= 30.0);
22454 if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate > 0.0)) || (usat &&
22455 (_rtB->Servorate < 0.0))) {
22456 _rtXdot->LimitedDeflection_CSTATE = _rtB->Servorate;
22457 } else {
22458 /* in saturation */
22459 _rtXdot->LimitedDeflection_CSTATE = 0.0;
22460 }
22461
22462 /* End of Derivatives for Integrator: '<S472>/Limited Deflection' */
22463
22464 /* Derivatives for Integrator: '<S473>/Servo rate' */
22465 _rtXdot->Servorate_CSTATE_n = _rtB->Sum2_o;
22466
22467 /* Derivatives for Integrator: '<S473>/Limited Deflection' */
22468 lsat = (_rtX->LimitedDeflection_CSTATE_i <= -30.0);
22469 usat = (_rtX->LimitedDeflection_CSTATE_i >= 30.0);
22470 if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_l > 0.0)) || (usat &&
22471 (_rtB->Servorate_l < 0.0))) {
22472 _rtXdot->LimitedDeflection_CSTATE_i = _rtB->Servorate_l;
22473 } else {
22474 /* in saturation */
22475 _rtXdot->LimitedDeflection_CSTATE_i = 0.0;
22476 }
22477
22478 /* End of Derivatives for Integrator: '<S473>/Limited Deflection' */
22479
22480 /* Derivatives for Integrator: '<S474>/Servo rate' */
22481 _rtXdot->Servorate_CSTATE_m = _rtB->Sum2_f;
22482
22483 /* Derivatives for Integrator: '<S474>/Limited Deflection' */
22484 lsat = (_rtX->LimitedDeflection_CSTATE_m <= -30.0);
22485 usat = (_rtX->LimitedDeflection_CSTATE_m >= 30.0);
22486 if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_lu > 0.0)) || (usat &&
22487 (_rtB->Servorate_lu < 0.0))) {
22488 _rtXdot->LimitedDeflection_CSTATE_m = _rtB->Servorate_lu;
22489 } else {
22490 /* in saturation */
22491 _rtXdot->LimitedDeflection_CSTATE_m = 0.0;
22492 }
22493
22494 /* End of Derivatives for Integrator: '<S474>/Limited Deflection' */
22495
22496 /* Derivatives for Integrator: '<S475>/Servo rate' */
22497 _rtXdot->Servorate_CSTATE_mi = _rtB->Sum2_i;
22498
22499 /* Derivatives for Integrator: '<S475>/Limited Deflection' */
22500 lsat = (_rtX->LimitedDeflection_CSTATE_f <= -30.0);
22501 usat = (_rtX->LimitedDeflection_CSTATE_f >= 30.0);
22502 if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_g > 0.0)) || (usat &&
22503 (_rtB->Servorate_g < 0.0))) {
22504 _rtXdot->LimitedDeflection_CSTATE_f = _rtB->Servorate_g;
22505 } else {
22506 /* in saturation */
22507 _rtXdot->LimitedDeflection_CSTATE_f = 0.0;
22508 }
22509
22510 /* End of Derivatives for Integrator: '<S475>/Limited Deflection' */
22511
22512 /* Derivatives for Integrator: '<S476>/Servo rate' */
22513 _rtXdot->Servorate_CSTATE_f = _rtB->Sum2_p;
22514
22515 /* Derivatives for Integrator: '<S476>/Limited Deflection' */
22516 lsat = (_rtX->LimitedDeflection_CSTATE_g <= -30.0);
22517 usat = (_rtX->LimitedDeflection_CSTATE_g >= 30.0);
22518 if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_m > 0.0)) || (usat &&
22519 (_rtB->Servorate_m < 0.0))) {
22520 _rtXdot->LimitedDeflection_CSTATE_g = _rtB->Servorate_m;
22521 } else {
22522 /* in saturation */
22523 _rtXdot->LimitedDeflection_CSTATE_g = 0.0;
22524 }
22525
22526 /* End of Derivatives for Integrator: '<S476>/Limited Deflection' */
22527
22528 /* Derivatives for Integrator: '<S477>/Servo rate' */
22529 _rtXdot->Servorate_CSTATE_k = _rtB->Sum2_m;
22530
22531 /* Derivatives for Integrator: '<S477>/Limited Deflection' */
22532 lsat = (_rtX->LimitedDeflection_CSTATE_l <= -30.0);
22533 usat = (_rtX->LimitedDeflection_CSTATE_l >= 30.0);
22534 if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_ga > 0.0)) || (usat &&
22535 (_rtB->Servorate_ga < 0.0))) {
22536 _rtXdot->LimitedDeflection_CSTATE_l = _rtB->Servorate_ga;
22537 } else {
22538 /* in saturation */
22539 _rtXdot->LimitedDeflection_CSTATE_l = 0.0;
22540 }
22541
22542 /* End of Derivatives for Integrator: '<S477>/Limited Deflection' */
22543
22544 /* Derivatives for Integrator: '<S478>/Servo rate' */
22545 _rtXdot->Servorate_CSTATE_h = _rtB->Sum2_k;
22546
22547 /* Derivatives for Integrator: '<S478>/Limited Deflection' */
22548 lsat = (_rtX->LimitedDeflection_CSTATE_j <= -30.0);
22549 usat = (_rtX->LimitedDeflection_CSTATE_j >= 30.0);
22550 if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_d > 0.0)) || (usat &&
22551 (_rtB->Servorate_d < 0.0))) {
22552 _rtXdot->LimitedDeflection_CSTATE_j = _rtB->Servorate_d;
22553 } else {
22554 /* in saturation */
22555 _rtXdot->LimitedDeflection_CSTATE_j = 0.0;
22556 }
22557
22558 /* End of Derivatives for Integrator: '<S478>/Limited Deflection' */
22559
22560 /* Derivatives for Integrator: '<S479>/Servo rate' */
22561 _rtXdot->Servorate_CSTATE_kx = _rtB->Sum2_mm;
22562
22563 /* Derivatives for Integrator: '<S479>/Limited Deflection' */
22564 lsat = (_rtX->LimitedDeflection_CSTATE_la <= -30.0);
22565 usat = (_rtX->LimitedDeflection_CSTATE_la >= 30.0);
22566 if (((!lsat) && (!usat)) || (lsat && (_rtB->Servorate_h > 0.0)) || (usat &&
22567 (_rtB->Servorate_h < 0.0))) {
22568 _rtXdot->LimitedDeflection_CSTATE_la = _rtB->Servorate_h;
22569 } else {
22570 /* in saturation */
22571 _rtXdot->LimitedDeflection_CSTATE_la = 0.0;
22572 }
22573
22574 /* End of Derivatives for Integrator: '<S479>/Limited Deflection' */
22575
22576 /* Derivatives for TransferFcn: '<S462>/TransferFcn1' */
22577 _rtXdot->TransferFcn1_CSTATE = 0.0;
22578 _rtXdot->TransferFcn1_CSTATE += -7.1710290426676231 *
22579 _rtX->TransferFcn1_CSTATE;
22580 _rtXdot->TransferFcn1_CSTATE += _rtB->DataTypeConversion4[0];
22581
22582 /* Derivatives for Integrator: '<S779>/q' */
22583 _rtXdot->q_CSTATE[0] = _rtB->q0dot;
22584 _rtXdot->q_CSTATE[1] = _rtB->q1dot;
22585 _rtXdot->q_CSTATE[2] = _rtB->q2dot;
22586 _rtXdot->q_CSTATE[3] = _rtB->q3dot;
22587
22588 /* Derivatives for Integrator: '<S778>/p1' */
22589 _rtXdot->p1_CSTATE[0] = _rtB->Product5[0];
22590
22591 /* Derivatives for Integrator: '<S780>/ub,vb,wb' */
22592 _rtXdot->ubvbwb_CSTATE[0] = _rtB->Sum_e[0];
22593
22594 /* Derivatives for Integrator: '<S470>/p,q,r ' */
22595 _rtXdot->pqr_CSTATE[0] = _rtB->Reshape[0];
22596
22597 /* Derivatives for Integrator: '<S778>/p1' */
22598 _rtXdot->p1_CSTATE[1] = _rtB->Product5[1];
22599
22600 /* Derivatives for Integrator: '<S780>/ub,vb,wb' */
22601 _rtXdot->ubvbwb_CSTATE[1] = _rtB->Sum_e[1];
22602
22603 /* Derivatives for Integrator: '<S470>/p,q,r ' */
22604 _rtXdot->pqr_CSTATE[1] = _rtB->Reshape[1];
22605
22606 /* Derivatives for Integrator: '<S778>/p1' */
22607 _rtXdot->p1_CSTATE[2] = _rtB->Product5[2];
22608
22609 /* Derivatives for Integrator: '<S780>/ub,vb,wb' */
22610 _rtXdot->ubvbwb_CSTATE[2] = _rtB->Sum_e[2];
22611
22612 /* Derivatives for Integrator: '<S470>/p,q,r ' */
22613 _rtXdot->pqr_CSTATE[2] = _rtB->Reshape[2];
22614
22615 /* Derivatives for Integrator: '<S683>/Integrator' */
22616 lsat = (_rtX->Integrator_CSTATE <= -8.0);
22617 usat = (_rtX->Integrator_CSTATE >= 8.0);
22618 if (((!lsat) && (!usat)) || (lsat && (_rtB->Saturation1 > 0.0)) || (usat &&
22619 (_rtB->Saturation1 < 0.0))) {
22620 _rtXdot->Integrator_CSTATE = _rtB->Saturation1;
22621 } else {
22622 /* in saturation */
22623 _rtXdot->Integrator_CSTATE = 0.0;
22624 }
22625
22626 /* End of Derivatives for Integrator: '<S683>/Integrator' */
22627
22628 /* Derivatives for Integrator: '<S682>/Integrator' */
22629 lsat = (_rtX->Integrator_CSTATE_l <= -30.0);
22630 usat = (_rtX->Integrator_CSTATE_l >= 30.0);
22631 if (((!lsat) && (!usat)) || (lsat && (_rtB->Saturation > 0.0)) || (usat &&
22632 (_rtB->Saturation < 0.0))) {
22633 _rtXdot->Integrator_CSTATE_l = _rtB->Saturation;
22634 } else {
22635 /* in saturation */
22636 _rtXdot->Integrator_CSTATE_l = 0.0;
22637 }
22638
22639 /* End of Derivatives for Integrator: '<S682>/Integrator' */
22640
22641 /* Derivatives for TransferFcn: '<S683>/Transfer Fcn2' */
22642 _rtXdot->TransferFcn2_CSTATE[0] = 0.0;
22643 _rtXdot->TransferFcn2_CSTATE[0] += -10.0 * _rtX->TransferFcn2_CSTATE[0];
22644 _rtXdot->TransferFcn2_CSTATE[1] = 0.0;
22645 _rtXdot->TransferFcn2_CSTATE[0] += -16.0 * _rtX->TransferFcn2_CSTATE[1];
22646 _rtXdot->TransferFcn2_CSTATE[1] += _rtX->TransferFcn2_CSTATE[0];
22647
22648 /* Derivatives for Integrator: '<S781>/Integrator' incorporates:
22649 * Constant: '<S781>/Constant1'
22650 */
22651 _rtXdot->Integrator_CSTATE_a = 7.292115E-5;
22652
22653 /* Derivatives for Integrator: '<S778>/p' */
22654 _rtXdot->p_CSTATE[0] = _rtB->Sum2_e[0];
22655 _rtXdot->p_CSTATE[1] = _rtB->Sum2_e[1];
22656 _rtXdot->p_CSTATE[2] = _rtB->Sum2_e[2];
22657
22658 /* Derivatives for TransferFcn: '<S446>/Transfer Fcn' */
22659 _rtXdot->TransferFcn_CSTATE = 0.0;
22660 _rtXdot->TransferFcn_CSTATE += -50.0 * _rtX->TransferFcn_CSTATE;
22661 _rtXdot->TransferFcn_CSTATE += _rtB->Switch_h;
22662 } else {
22663 {
22664 real_T *dx;
22665 int_T i;
22666 dx = &(((XDot_AFCS_MODEL1_T *) ssGetdX(S))->Servorate_CSTATE);
22667 for (i=0; i < 39; i++) {
22668 dx[i] = 0.0;
22669 }
22670 }
22671 }
22672
22673 /* End of Derivatives for SubSystem: '<S15>/Subsystem3' */
22674 /* End of Derivatives for SubSystem: '<S1>/Subsystem5' */
22675}
22676
22677/* Termination for root system: '<Root>' */
22678static void mdlTerminate(SimStruct *S)
22679{
22680 B_AFCS_MODEL1_T *_rtB;
22681 _rtB = ((B_AFCS_MODEL1_T *) ssGetLocalBlockIO(S));
22682
22683 /* RTW Generated Level2 S-Function Block: '<S306>/Generated S-Function15' (StickGradientDeadZone_sf) */
22684 {
22685 SimStruct *rts = ssGetSFunction(S, 167);
22686 sfcnTerminate(rts);
22687 }
22688
22689 /* RTW Generated Level2 S-Function Block: '<S305>/Generated S-Function15' (StickGradientDeadZone_sf) */
22690 {
22691 SimStruct *rts = ssGetSFunction(S, 168);
22692 sfcnTerminate(rts);
22693 }
22694
22695 /* RTW Generated Level2 S-Function Block: '<S304>/Generated S-Function15' (StickGradientDeadZone_sf) */
22696 {
22697 SimStruct *rts = ssGetSFunction(S, 169);
22698 sfcnTerminate(rts);
22699 }
22700
22701 /* Terminate for Atomic SubSystem: '<S7>/Level' */
22702
22703 /* Terminate for Atomic SubSystem: '<S22>/CodeReuseSubsystem' */
22704 AFCS_MODEL1_CodeReuseSubsystem_Term(S);
22705
22706 /* End of Terminate for SubSystem: '<S22>/CodeReuseSubsystem' */
22707
22708 /* End of Terminate for SubSystem: '<S7>/Level' */
22709#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)
22710
22711 /* child S-function code */
22712 if (_ssGetSFunctions(S) ) {
22713 /* RTW Generated Level2 S-Function Block: '<S174>/Generated S-Function' (LagFilter_sf) */
22714 {
22715 SimStruct *rts = ssGetSFunction(S, 0);
22716 if (rts != (NULL) ) {
22717 rt_FREE(ssGetSampleTimePtr(rts));
22718 rt_FREE(ssGetOffsetTimePtr(rts));
22719 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
22720
22721 /* Destroy model methods 3 */
22722 {
22723 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
22724 rt_FREE(methods3);
22725 }
22726
22727 /* Destroy model methods 2 */
22728 {
22729 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
22730 rt_FREE(methods2);
22731 }
22732
22733 /* Destroy state auxilliary information */
22734 {
22735 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
22736 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
22737 rt_FREE(periodicStatesInfo);
22738 rt_FREE(statesInfo2);
22739 }
22740
22741 {
22742 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
22743 rt_FREE(ptr);
22744 }
22745
22746 {
22747 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
22748 rt_FREE(ptr);
22749 }
22750
22751 {
22752 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
22753 rt_FREE(ptr);
22754 }
22755
22756 {
22757 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
22758 rt_FREE(ptr);
22759 }
22760
22761 {
22762 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
22763 rt_FREE(ptr);
22764 }
22765
22766 {
22767 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
22768 rt_FREE(ptr);
22769 }
22770
22771 rt_FREE(ssGetPortInfoForInputs(rts));
22772 rt_FREE(ssGetPortInfoForOutputs(rts));
22773 rt_FREE(ssGetSFcnDWork(rts));
22774 }
22775 }
22776
22777 /* RTW Generated Level2 S-Function Block: '<S186>/Generated S-Function' (SaturationLimiter_sf) */
22778 {
22779 SimStruct *rts = ssGetSFunction(S, 1);
22780 if (rts != (NULL) ) {
22781 rt_FREE(ssGetSampleTimePtr(rts));
22782 rt_FREE(ssGetOffsetTimePtr(rts));
22783 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
22784
22785 /* Destroy model methods 3 */
22786 {
22787 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
22788 rt_FREE(methods3);
22789 }
22790
22791 /* Destroy model methods 2 */
22792 {
22793 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
22794 rt_FREE(methods2);
22795 }
22796
22797 /* Destroy state auxilliary information */
22798 {
22799 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
22800 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
22801 rt_FREE(periodicStatesInfo);
22802 rt_FREE(statesInfo2);
22803 }
22804
22805 {
22806 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
22807 rt_FREE(ptr);
22808 }
22809
22810 {
22811 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
22812 rt_FREE(ptr);
22813 }
22814
22815 {
22816 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
22817 rt_FREE(ptr);
22818 }
22819
22820 rt_FREE(ssGetPortInfoForInputs(rts));
22821 rt_FREE(ssGetPortInfoForOutputs(rts));
22822 }
22823 }
22824
22825 /* RTW Generated Level2 S-Function Block: '<S188>/Generated S-Function' (Gain_sf) */
22826 {
22827 SimStruct *rts = ssGetSFunction(S, 2);
22828 if (rts != (NULL) ) {
22829 rt_FREE(ssGetSampleTimePtr(rts));
22830 rt_FREE(ssGetOffsetTimePtr(rts));
22831 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
22832
22833 /* Destroy model methods 3 */
22834 {
22835 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
22836 rt_FREE(methods3);
22837 }
22838
22839 /* Destroy model methods 2 */
22840 {
22841 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
22842 rt_FREE(methods2);
22843 }
22844
22845 /* Destroy state auxilliary information */
22846 {
22847 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
22848 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
22849 rt_FREE(periodicStatesInfo);
22850 rt_FREE(statesInfo2);
22851 }
22852
22853 {
22854 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
22855 rt_FREE(ptr);
22856 }
22857
22858 {
22859 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
22860 rt_FREE(ptr);
22861 }
22862
22863 rt_FREE(ssGetPortInfoForInputs(rts));
22864 rt_FREE(ssGetPortInfoForOutputs(rts));
22865 }
22866 }
22867
22868 /* RTW Generated Level2 S-Function Block: '<S185>/Generated S-Function' (LeadLagFilter_sf) */
22869 {
22870 SimStruct *rts = ssGetSFunction(S, 3);
22871 if (rts != (NULL) ) {
22872 rt_FREE(ssGetSampleTimePtr(rts));
22873 rt_FREE(ssGetOffsetTimePtr(rts));
22874 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
22875
22876 /* Destroy model methods 3 */
22877 {
22878 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
22879 rt_FREE(methods3);
22880 }
22881
22882 /* Destroy model methods 2 */
22883 {
22884 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
22885 rt_FREE(methods2);
22886 }
22887
22888 /* Destroy state auxilliary information */
22889 {
22890 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
22891 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
22892 rt_FREE(periodicStatesInfo);
22893 rt_FREE(statesInfo2);
22894 }
22895
22896 {
22897 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
22898 rt_FREE(ptr);
22899 }
22900
22901 {
22902 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
22903 rt_FREE(ptr);
22904 }
22905
22906 {
22907 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
22908 rt_FREE(ptr);
22909 }
22910
22911 {
22912 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
22913 rt_FREE(ptr);
22914 }
22915
22916 {
22917 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
22918 rt_FREE(ptr);
22919 }
22920
22921 {
22922 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
22923 rt_FREE(ptr);
22924 }
22925
22926 {
22927 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
22928 rt_FREE(ptr);
22929 }
22930
22931 rt_FREE(ssGetPortInfoForInputs(rts));
22932 rt_FREE(ssGetPortInfoForOutputs(rts));
22933 rt_FREE(ssGetSFcnDWork(rts));
22934 }
22935 }
22936
22937 /* RTW Generated Level2 S-Function Block: '<S187>/Generated S-Function' (SaturationLimiter_sf) */
22938 {
22939 SimStruct *rts = ssGetSFunction(S, 4);
22940 if (rts != (NULL) ) {
22941 rt_FREE(ssGetSampleTimePtr(rts));
22942 rt_FREE(ssGetOffsetTimePtr(rts));
22943 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
22944
22945 /* Destroy model methods 3 */
22946 {
22947 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
22948 rt_FREE(methods3);
22949 }
22950
22951 /* Destroy model methods 2 */
22952 {
22953 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
22954 rt_FREE(methods2);
22955 }
22956
22957 /* Destroy state auxilliary information */
22958 {
22959 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
22960 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
22961 rt_FREE(periodicStatesInfo);
22962 rt_FREE(statesInfo2);
22963 }
22964
22965 {
22966 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
22967 rt_FREE(ptr);
22968 }
22969
22970 {
22971 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
22972 rt_FREE(ptr);
22973 }
22974
22975 {
22976 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
22977 rt_FREE(ptr);
22978 }
22979
22980 rt_FREE(ssGetPortInfoForInputs(rts));
22981 rt_FREE(ssGetPortInfoForOutputs(rts));
22982 }
22983 }
22984
22985 /* RTW Generated Level2 S-Function Block: '<S189>/Generated S-Function' (Gain_sf) */
22986 {
22987 SimStruct *rts = ssGetSFunction(S, 5);
22988 if (rts != (NULL) ) {
22989 rt_FREE(ssGetSampleTimePtr(rts));
22990 rt_FREE(ssGetOffsetTimePtr(rts));
22991 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
22992
22993 /* Destroy model methods 3 */
22994 {
22995 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
22996 rt_FREE(methods3);
22997 }
22998
22999 /* Destroy model methods 2 */
23000 {
23001 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23002 rt_FREE(methods2);
23003 }
23004
23005 /* Destroy state auxilliary information */
23006 {
23007 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23008 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23009 rt_FREE(periodicStatesInfo);
23010 rt_FREE(statesInfo2);
23011 }
23012
23013 {
23014 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23015 rt_FREE(ptr);
23016 }
23017
23018 {
23019 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23020 rt_FREE(ptr);
23021 }
23022
23023 rt_FREE(ssGetPortInfoForInputs(rts));
23024 rt_FREE(ssGetPortInfoForOutputs(rts));
23025 }
23026 }
23027
23028 /* RTW Generated Level2 S-Function Block: '<S184>/Generated S-Function' (IntegratorLimited_sf) */
23029 {
23030 SimStruct *rts = ssGetSFunction(S, 6);
23031 if (rts != (NULL) ) {
23032 rt_FREE(ssGetSampleTimePtr(rts));
23033 rt_FREE(ssGetOffsetTimePtr(rts));
23034 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23035
23036 /* Destroy model methods 3 */
23037 {
23038 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23039 rt_FREE(methods3);
23040 }
23041
23042 /* Destroy model methods 2 */
23043 {
23044 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23045 rt_FREE(methods2);
23046 }
23047
23048 /* Destroy state auxilliary information */
23049 {
23050 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23051 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23052 rt_FREE(periodicStatesInfo);
23053 rt_FREE(statesInfo2);
23054 }
23055
23056 {
23057 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23058 rt_FREE(ptr);
23059 }
23060
23061 {
23062 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23063 rt_FREE(ptr);
23064 }
23065
23066 {
23067 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23068 rt_FREE(ptr);
23069 }
23070
23071 {
23072 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
23073 rt_FREE(ptr);
23074 }
23075
23076 {
23077 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
23078 rt_FREE(ptr);
23079 }
23080
23081 {
23082 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
23083 rt_FREE(ptr);
23084 }
23085
23086 {
23087 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
23088 rt_FREE(ptr);
23089 }
23090
23091 rt_FREE(ssGetPortInfoForInputs(rts));
23092 rt_FREE(ssGetPortInfoForOutputs(rts));
23093 rt_FREE(ssGetSFcnDWork(rts));
23094 }
23095 }
23096
23097 /* RTW Generated Level2 S-Function Block: '<S183>/Generated S-Function' (LagFilter_sf) */
23098 {
23099 SimStruct *rts = ssGetSFunction(S, 7);
23100 if (rts != (NULL) ) {
23101 rt_FREE(ssGetSampleTimePtr(rts));
23102 rt_FREE(ssGetOffsetTimePtr(rts));
23103 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23104
23105 /* Destroy model methods 3 */
23106 {
23107 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23108 rt_FREE(methods3);
23109 }
23110
23111 /* Destroy model methods 2 */
23112 {
23113 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23114 rt_FREE(methods2);
23115 }
23116
23117 /* Destroy state auxilliary information */
23118 {
23119 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23120 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23121 rt_FREE(periodicStatesInfo);
23122 rt_FREE(statesInfo2);
23123 }
23124
23125 {
23126 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23127 rt_FREE(ptr);
23128 }
23129
23130 {
23131 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23132 rt_FREE(ptr);
23133 }
23134
23135 {
23136 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23137 rt_FREE(ptr);
23138 }
23139
23140 {
23141 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
23142 rt_FREE(ptr);
23143 }
23144
23145 {
23146 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
23147 rt_FREE(ptr);
23148 }
23149
23150 {
23151 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
23152 rt_FREE(ptr);
23153 }
23154
23155 rt_FREE(ssGetPortInfoForInputs(rts));
23156 rt_FREE(ssGetPortInfoForOutputs(rts));
23157 rt_FREE(ssGetSFcnDWork(rts));
23158 }
23159 }
23160
23161 /* RTW Generated Level2 S-Function Block: '<S179>/Generated S-Function' (SaturationLimiter_sf) */
23162 {
23163 SimStruct *rts = ssGetSFunction(S, 8);
23164 if (rts != (NULL) ) {
23165 rt_FREE(ssGetSampleTimePtr(rts));
23166 rt_FREE(ssGetOffsetTimePtr(rts));
23167 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23168
23169 /* Destroy model methods 3 */
23170 {
23171 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23172 rt_FREE(methods3);
23173 }
23174
23175 /* Destroy model methods 2 */
23176 {
23177 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23178 rt_FREE(methods2);
23179 }
23180
23181 /* Destroy state auxilliary information */
23182 {
23183 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23184 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23185 rt_FREE(periodicStatesInfo);
23186 rt_FREE(statesInfo2);
23187 }
23188
23189 {
23190 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23191 rt_FREE(ptr);
23192 }
23193
23194 {
23195 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23196 rt_FREE(ptr);
23197 }
23198
23199 {
23200 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23201 rt_FREE(ptr);
23202 }
23203
23204 rt_FREE(ssGetPortInfoForInputs(rts));
23205 rt_FREE(ssGetPortInfoForOutputs(rts));
23206 }
23207 }
23208
23209 /* RTW Generated Level2 S-Function Block: '<S181>/Generated S-Function' (Gain_sf) */
23210 {
23211 SimStruct *rts = ssGetSFunction(S, 9);
23212 if (rts != (NULL) ) {
23213 rt_FREE(ssGetSampleTimePtr(rts));
23214 rt_FREE(ssGetOffsetTimePtr(rts));
23215 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23216
23217 /* Destroy model methods 3 */
23218 {
23219 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23220 rt_FREE(methods3);
23221 }
23222
23223 /* Destroy model methods 2 */
23224 {
23225 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23226 rt_FREE(methods2);
23227 }
23228
23229 /* Destroy state auxilliary information */
23230 {
23231 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23232 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23233 rt_FREE(periodicStatesInfo);
23234 rt_FREE(statesInfo2);
23235 }
23236
23237 {
23238 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23239 rt_FREE(ptr);
23240 }
23241
23242 {
23243 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23244 rt_FREE(ptr);
23245 }
23246
23247 rt_FREE(ssGetPortInfoForInputs(rts));
23248 rt_FREE(ssGetPortInfoForOutputs(rts));
23249 }
23250 }
23251
23252 /* RTW Generated Level2 S-Function Block: '<S178>/Generated S-Function' (LeadLagFilter_sf) */
23253 {
23254 SimStruct *rts = ssGetSFunction(S, 10);
23255 if (rts != (NULL) ) {
23256 rt_FREE(ssGetSampleTimePtr(rts));
23257 rt_FREE(ssGetOffsetTimePtr(rts));
23258 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23259
23260 /* Destroy model methods 3 */
23261 {
23262 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23263 rt_FREE(methods3);
23264 }
23265
23266 /* Destroy model methods 2 */
23267 {
23268 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23269 rt_FREE(methods2);
23270 }
23271
23272 /* Destroy state auxilliary information */
23273 {
23274 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23275 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23276 rt_FREE(periodicStatesInfo);
23277 rt_FREE(statesInfo2);
23278 }
23279
23280 {
23281 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23282 rt_FREE(ptr);
23283 }
23284
23285 {
23286 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23287 rt_FREE(ptr);
23288 }
23289
23290 {
23291 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23292 rt_FREE(ptr);
23293 }
23294
23295 {
23296 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
23297 rt_FREE(ptr);
23298 }
23299
23300 {
23301 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
23302 rt_FREE(ptr);
23303 }
23304
23305 {
23306 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
23307 rt_FREE(ptr);
23308 }
23309
23310 {
23311 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
23312 rt_FREE(ptr);
23313 }
23314
23315 rt_FREE(ssGetPortInfoForInputs(rts));
23316 rt_FREE(ssGetPortInfoForOutputs(rts));
23317 rt_FREE(ssGetSFcnDWork(rts));
23318 }
23319 }
23320
23321 /* RTW Generated Level2 S-Function Block: '<S180>/Generated S-Function' (SaturationLimiter_sf) */
23322 {
23323 SimStruct *rts = ssGetSFunction(S, 11);
23324 if (rts != (NULL) ) {
23325 rt_FREE(ssGetSampleTimePtr(rts));
23326 rt_FREE(ssGetOffsetTimePtr(rts));
23327 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23328
23329 /* Destroy model methods 3 */
23330 {
23331 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23332 rt_FREE(methods3);
23333 }
23334
23335 /* Destroy model methods 2 */
23336 {
23337 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23338 rt_FREE(methods2);
23339 }
23340
23341 /* Destroy state auxilliary information */
23342 {
23343 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23344 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23345 rt_FREE(periodicStatesInfo);
23346 rt_FREE(statesInfo2);
23347 }
23348
23349 {
23350 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23351 rt_FREE(ptr);
23352 }
23353
23354 {
23355 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23356 rt_FREE(ptr);
23357 }
23358
23359 {
23360 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23361 rt_FREE(ptr);
23362 }
23363
23364 rt_FREE(ssGetPortInfoForInputs(rts));
23365 rt_FREE(ssGetPortInfoForOutputs(rts));
23366 }
23367 }
23368
23369 /* RTW Generated Level2 S-Function Block: '<S182>/Generated S-Function' (Gain_sf) */
23370 {
23371 SimStruct *rts = ssGetSFunction(S, 12);
23372 if (rts != (NULL) ) {
23373 rt_FREE(ssGetSampleTimePtr(rts));
23374 rt_FREE(ssGetOffsetTimePtr(rts));
23375 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23376
23377 /* Destroy model methods 3 */
23378 {
23379 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23380 rt_FREE(methods3);
23381 }
23382
23383 /* Destroy model methods 2 */
23384 {
23385 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23386 rt_FREE(methods2);
23387 }
23388
23389 /* Destroy state auxilliary information */
23390 {
23391 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23392 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23393 rt_FREE(periodicStatesInfo);
23394 rt_FREE(statesInfo2);
23395 }
23396
23397 {
23398 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23399 rt_FREE(ptr);
23400 }
23401
23402 {
23403 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23404 rt_FREE(ptr);
23405 }
23406
23407 rt_FREE(ssGetPortInfoForInputs(rts));
23408 rt_FREE(ssGetPortInfoForOutputs(rts));
23409 }
23410 }
23411
23412 /* RTW Generated Level2 S-Function Block: '<S177>/Generated S-Function' (IntegratorLimited_sf) */
23413 {
23414 SimStruct *rts = ssGetSFunction(S, 13);
23415 if (rts != (NULL) ) {
23416 rt_FREE(ssGetSampleTimePtr(rts));
23417 rt_FREE(ssGetOffsetTimePtr(rts));
23418 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23419
23420 /* Destroy model methods 3 */
23421 {
23422 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23423 rt_FREE(methods3);
23424 }
23425
23426 /* Destroy model methods 2 */
23427 {
23428 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23429 rt_FREE(methods2);
23430 }
23431
23432 /* Destroy state auxilliary information */
23433 {
23434 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23435 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23436 rt_FREE(periodicStatesInfo);
23437 rt_FREE(statesInfo2);
23438 }
23439
23440 {
23441 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23442 rt_FREE(ptr);
23443 }
23444
23445 {
23446 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23447 rt_FREE(ptr);
23448 }
23449
23450 {
23451 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23452 rt_FREE(ptr);
23453 }
23454
23455 {
23456 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
23457 rt_FREE(ptr);
23458 }
23459
23460 {
23461 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
23462 rt_FREE(ptr);
23463 }
23464
23465 {
23466 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
23467 rt_FREE(ptr);
23468 }
23469
23470 {
23471 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
23472 rt_FREE(ptr);
23473 }
23474
23475 rt_FREE(ssGetPortInfoForInputs(rts));
23476 rt_FREE(ssGetPortInfoForOutputs(rts));
23477 rt_FREE(ssGetSFcnDWork(rts));
23478 }
23479 }
23480
23481 /* RTW Generated Level2 S-Function Block: '<S176>/Generated S-Function' (LagFilter_sf) */
23482 {
23483 SimStruct *rts = ssGetSFunction(S, 14);
23484 if (rts != (NULL) ) {
23485 rt_FREE(ssGetSampleTimePtr(rts));
23486 rt_FREE(ssGetOffsetTimePtr(rts));
23487 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23488
23489 /* Destroy model methods 3 */
23490 {
23491 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23492 rt_FREE(methods3);
23493 }
23494
23495 /* Destroy model methods 2 */
23496 {
23497 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23498 rt_FREE(methods2);
23499 }
23500
23501 /* Destroy state auxilliary information */
23502 {
23503 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23504 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23505 rt_FREE(periodicStatesInfo);
23506 rt_FREE(statesInfo2);
23507 }
23508
23509 {
23510 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23511 rt_FREE(ptr);
23512 }
23513
23514 {
23515 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23516 rt_FREE(ptr);
23517 }
23518
23519 {
23520 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23521 rt_FREE(ptr);
23522 }
23523
23524 {
23525 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
23526 rt_FREE(ptr);
23527 }
23528
23529 {
23530 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
23531 rt_FREE(ptr);
23532 }
23533
23534 {
23535 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
23536 rt_FREE(ptr);
23537 }
23538
23539 rt_FREE(ssGetPortInfoForInputs(rts));
23540 rt_FREE(ssGetPortInfoForOutputs(rts));
23541 rt_FREE(ssGetSFcnDWork(rts));
23542 }
23543 }
23544
23545 /* RTW Generated Level2 S-Function Block: '<S92>/Generated S-Function1' (SaturationLimiter_sf) */
23546 {
23547 SimStruct *rts = ssGetSFunction(S, 15);
23548 if (rts != (NULL) ) {
23549 rt_FREE(ssGetSampleTimePtr(rts));
23550 rt_FREE(ssGetOffsetTimePtr(rts));
23551 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23552
23553 /* Destroy model methods 3 */
23554 {
23555 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23556 rt_FREE(methods3);
23557 }
23558
23559 /* Destroy model methods 2 */
23560 {
23561 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23562 rt_FREE(methods2);
23563 }
23564
23565 /* Destroy state auxilliary information */
23566 {
23567 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23568 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23569 rt_FREE(periodicStatesInfo);
23570 rt_FREE(statesInfo2);
23571 }
23572
23573 {
23574 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23575 rt_FREE(ptr);
23576 }
23577
23578 {
23579 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23580 rt_FREE(ptr);
23581 }
23582
23583 {
23584 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23585 rt_FREE(ptr);
23586 }
23587
23588 rt_FREE(ssGetPortInfoForInputs(rts));
23589 rt_FREE(ssGetPortInfoForOutputs(rts));
23590 }
23591 }
23592
23593 /* RTW Generated Level2 S-Function Block: '<S96>/Generated S-Function' (Gain_sf) */
23594 {
23595 SimStruct *rts = ssGetSFunction(S, 16);
23596 if (rts != (NULL) ) {
23597 rt_FREE(ssGetSampleTimePtr(rts));
23598 rt_FREE(ssGetOffsetTimePtr(rts));
23599 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23600
23601 /* Destroy model methods 3 */
23602 {
23603 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23604 rt_FREE(methods3);
23605 }
23606
23607 /* Destroy model methods 2 */
23608 {
23609 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23610 rt_FREE(methods2);
23611 }
23612
23613 /* Destroy state auxilliary information */
23614 {
23615 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23616 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23617 rt_FREE(periodicStatesInfo);
23618 rt_FREE(statesInfo2);
23619 }
23620
23621 {
23622 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23623 rt_FREE(ptr);
23624 }
23625
23626 {
23627 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23628 rt_FREE(ptr);
23629 }
23630
23631 rt_FREE(ssGetPortInfoForInputs(rts));
23632 rt_FREE(ssGetPortInfoForOutputs(rts));
23633 }
23634 }
23635
23636 /* RTW Generated Level2 S-Function Block: '<S93>/Generated S-Function1' (SaturationLimiter_sf) */
23637 {
23638 SimStruct *rts = ssGetSFunction(S, 17);
23639 if (rts != (NULL) ) {
23640 rt_FREE(ssGetSampleTimePtr(rts));
23641 rt_FREE(ssGetOffsetTimePtr(rts));
23642 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23643
23644 /* Destroy model methods 3 */
23645 {
23646 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23647 rt_FREE(methods3);
23648 }
23649
23650 /* Destroy model methods 2 */
23651 {
23652 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23653 rt_FREE(methods2);
23654 }
23655
23656 /* Destroy state auxilliary information */
23657 {
23658 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23659 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23660 rt_FREE(periodicStatesInfo);
23661 rt_FREE(statesInfo2);
23662 }
23663
23664 {
23665 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23666 rt_FREE(ptr);
23667 }
23668
23669 {
23670 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23671 rt_FREE(ptr);
23672 }
23673
23674 {
23675 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23676 rt_FREE(ptr);
23677 }
23678
23679 rt_FREE(ssGetPortInfoForInputs(rts));
23680 rt_FREE(ssGetPortInfoForOutputs(rts));
23681 }
23682 }
23683
23684 /* RTW Generated Level2 S-Function Block: '<S97>/Generated S-Function' (Gain_sf) */
23685 {
23686 SimStruct *rts = ssGetSFunction(S, 18);
23687 if (rts != (NULL) ) {
23688 rt_FREE(ssGetSampleTimePtr(rts));
23689 rt_FREE(ssGetOffsetTimePtr(rts));
23690 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23691
23692 /* Destroy model methods 3 */
23693 {
23694 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23695 rt_FREE(methods3);
23696 }
23697
23698 /* Destroy model methods 2 */
23699 {
23700 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23701 rt_FREE(methods2);
23702 }
23703
23704 /* Destroy state auxilliary information */
23705 {
23706 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23707 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23708 rt_FREE(periodicStatesInfo);
23709 rt_FREE(statesInfo2);
23710 }
23711
23712 {
23713 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23714 rt_FREE(ptr);
23715 }
23716
23717 {
23718 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23719 rt_FREE(ptr);
23720 }
23721
23722 rt_FREE(ssGetPortInfoForInputs(rts));
23723 rt_FREE(ssGetPortInfoForOutputs(rts));
23724 }
23725 }
23726
23727 /* RTW Generated Level2 S-Function Block: '<S89>/Generated S-Function3' (IntegratorLimited_sf) */
23728 {
23729 SimStruct *rts = ssGetSFunction(S, 19);
23730 if (rts != (NULL) ) {
23731 rt_FREE(ssGetSampleTimePtr(rts));
23732 rt_FREE(ssGetOffsetTimePtr(rts));
23733 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23734
23735 /* Destroy model methods 3 */
23736 {
23737 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23738 rt_FREE(methods3);
23739 }
23740
23741 /* Destroy model methods 2 */
23742 {
23743 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23744 rt_FREE(methods2);
23745 }
23746
23747 /* Destroy state auxilliary information */
23748 {
23749 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23750 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23751 rt_FREE(periodicStatesInfo);
23752 rt_FREE(statesInfo2);
23753 }
23754
23755 {
23756 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23757 rt_FREE(ptr);
23758 }
23759
23760 {
23761 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23762 rt_FREE(ptr);
23763 }
23764
23765 {
23766 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23767 rt_FREE(ptr);
23768 }
23769
23770 {
23771 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
23772 rt_FREE(ptr);
23773 }
23774
23775 {
23776 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
23777 rt_FREE(ptr);
23778 }
23779
23780 {
23781 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
23782 rt_FREE(ptr);
23783 }
23784
23785 {
23786 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
23787 rt_FREE(ptr);
23788 }
23789
23790 rt_FREE(ssGetPortInfoForInputs(rts));
23791 rt_FREE(ssGetPortInfoForOutputs(rts));
23792 rt_FREE(ssGetSFcnDWork(rts));
23793 }
23794 }
23795
23796 /* RTW Generated Level2 S-Function Block: '<S91>/Generated S-Function1' (SaturationLimiter_sf) */
23797 {
23798 SimStruct *rts = ssGetSFunction(S, 20);
23799 if (rts != (NULL) ) {
23800 rt_FREE(ssGetSampleTimePtr(rts));
23801 rt_FREE(ssGetOffsetTimePtr(rts));
23802 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23803
23804 /* Destroy model methods 3 */
23805 {
23806 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23807 rt_FREE(methods3);
23808 }
23809
23810 /* Destroy model methods 2 */
23811 {
23812 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23813 rt_FREE(methods2);
23814 }
23815
23816 /* Destroy state auxilliary information */
23817 {
23818 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23819 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23820 rt_FREE(periodicStatesInfo);
23821 rt_FREE(statesInfo2);
23822 }
23823
23824 {
23825 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23826 rt_FREE(ptr);
23827 }
23828
23829 {
23830 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23831 rt_FREE(ptr);
23832 }
23833
23834 {
23835 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23836 rt_FREE(ptr);
23837 }
23838
23839 rt_FREE(ssGetPortInfoForInputs(rts));
23840 rt_FREE(ssGetPortInfoForOutputs(rts));
23841 }
23842 }
23843
23844 /* RTW Generated Level2 S-Function Block: '<S95>/Generated S-Function' (Gain_sf) */
23845 {
23846 SimStruct *rts = ssGetSFunction(S, 21);
23847 if (rts != (NULL) ) {
23848 rt_FREE(ssGetSampleTimePtr(rts));
23849 rt_FREE(ssGetOffsetTimePtr(rts));
23850 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23851
23852 /* Destroy model methods 3 */
23853 {
23854 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23855 rt_FREE(methods3);
23856 }
23857
23858 /* Destroy model methods 2 */
23859 {
23860 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23861 rt_FREE(methods2);
23862 }
23863
23864 /* Destroy state auxilliary information */
23865 {
23866 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23867 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23868 rt_FREE(periodicStatesInfo);
23869 rt_FREE(statesInfo2);
23870 }
23871
23872 {
23873 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23874 rt_FREE(ptr);
23875 }
23876
23877 {
23878 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23879 rt_FREE(ptr);
23880 }
23881
23882 rt_FREE(ssGetPortInfoForInputs(rts));
23883 rt_FREE(ssGetPortInfoForOutputs(rts));
23884 }
23885 }
23886
23887 /* RTW Generated Level2 S-Function Block: '<S90>/Generated S-Function2' (LeadLagFilter_sf) */
23888 {
23889 SimStruct *rts = ssGetSFunction(S, 22);
23890 if (rts != (NULL) ) {
23891 rt_FREE(ssGetSampleTimePtr(rts));
23892 rt_FREE(ssGetOffsetTimePtr(rts));
23893 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23894
23895 /* Destroy model methods 3 */
23896 {
23897 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23898 rt_FREE(methods3);
23899 }
23900
23901 /* Destroy model methods 2 */
23902 {
23903 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23904 rt_FREE(methods2);
23905 }
23906
23907 /* Destroy state auxilliary information */
23908 {
23909 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23910 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23911 rt_FREE(periodicStatesInfo);
23912 rt_FREE(statesInfo2);
23913 }
23914
23915 {
23916 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23917 rt_FREE(ptr);
23918 }
23919
23920 {
23921 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23922 rt_FREE(ptr);
23923 }
23924
23925 {
23926 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23927 rt_FREE(ptr);
23928 }
23929
23930 {
23931 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
23932 rt_FREE(ptr);
23933 }
23934
23935 {
23936 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
23937 rt_FREE(ptr);
23938 }
23939
23940 {
23941 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
23942 rt_FREE(ptr);
23943 }
23944
23945 {
23946 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
23947 rt_FREE(ptr);
23948 }
23949
23950 rt_FREE(ssGetPortInfoForInputs(rts));
23951 rt_FREE(ssGetPortInfoForOutputs(rts));
23952 rt_FREE(ssGetSFcnDWork(rts));
23953 }
23954 }
23955
23956 /* RTW Generated Level2 S-Function Block: '<S94>/Generated S-Function1' (SaturationLimiter_sf) */
23957 {
23958 SimStruct *rts = ssGetSFunction(S, 23);
23959 if (rts != (NULL) ) {
23960 rt_FREE(ssGetSampleTimePtr(rts));
23961 rt_FREE(ssGetOffsetTimePtr(rts));
23962 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
23963
23964 /* Destroy model methods 3 */
23965 {
23966 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
23967 rt_FREE(methods3);
23968 }
23969
23970 /* Destroy model methods 2 */
23971 {
23972 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
23973 rt_FREE(methods2);
23974 }
23975
23976 /* Destroy state auxilliary information */
23977 {
23978 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
23979 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
23980 rt_FREE(periodicStatesInfo);
23981 rt_FREE(statesInfo2);
23982 }
23983
23984 {
23985 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
23986 rt_FREE(ptr);
23987 }
23988
23989 {
23990 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
23991 rt_FREE(ptr);
23992 }
23993
23994 {
23995 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
23996 rt_FREE(ptr);
23997 }
23998
23999 rt_FREE(ssGetPortInfoForInputs(rts));
24000 rt_FREE(ssGetPortInfoForOutputs(rts));
24001 }
24002 }
24003
24004 /* RTW Generated Level2 S-Function Block: '<S87>/Generated S-Function4' (LagFilter_sf) */
24005 {
24006 SimStruct *rts = ssGetSFunction(S, 24);
24007 if (rts != (NULL) ) {
24008 rt_FREE(ssGetSampleTimePtr(rts));
24009 rt_FREE(ssGetOffsetTimePtr(rts));
24010 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24011
24012 /* Destroy model methods 3 */
24013 {
24014 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24015 rt_FREE(methods3);
24016 }
24017
24018 /* Destroy model methods 2 */
24019 {
24020 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24021 rt_FREE(methods2);
24022 }
24023
24024 /* Destroy state auxilliary information */
24025 {
24026 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24027 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24028 rt_FREE(periodicStatesInfo);
24029 rt_FREE(statesInfo2);
24030 }
24031
24032 {
24033 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24034 rt_FREE(ptr);
24035 }
24036
24037 {
24038 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24039 rt_FREE(ptr);
24040 }
24041
24042 {
24043 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24044 rt_FREE(ptr);
24045 }
24046
24047 {
24048 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
24049 rt_FREE(ptr);
24050 }
24051
24052 {
24053 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
24054 rt_FREE(ptr);
24055 }
24056
24057 {
24058 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
24059 rt_FREE(ptr);
24060 }
24061
24062 rt_FREE(ssGetPortInfoForInputs(rts));
24063 rt_FREE(ssGetPortInfoForOutputs(rts));
24064 rt_FREE(ssGetSFcnDWork(rts));
24065 }
24066 }
24067
24068 /* RTW Generated Level2 S-Function Block: '<S114>/Generated S-Function' (SaturationLimiter_sf) */
24069 {
24070 SimStruct *rts = ssGetSFunction(S, 25);
24071 if (rts != (NULL) ) {
24072 rt_FREE(ssGetSampleTimePtr(rts));
24073 rt_FREE(ssGetOffsetTimePtr(rts));
24074 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24075
24076 /* Destroy model methods 3 */
24077 {
24078 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24079 rt_FREE(methods3);
24080 }
24081
24082 /* Destroy model methods 2 */
24083 {
24084 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24085 rt_FREE(methods2);
24086 }
24087
24088 /* Destroy state auxilliary information */
24089 {
24090 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24091 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24092 rt_FREE(periodicStatesInfo);
24093 rt_FREE(statesInfo2);
24094 }
24095
24096 {
24097 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24098 rt_FREE(ptr);
24099 }
24100
24101 {
24102 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24103 rt_FREE(ptr);
24104 }
24105
24106 {
24107 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24108 rt_FREE(ptr);
24109 }
24110
24111 rt_FREE(ssGetPortInfoForInputs(rts));
24112 rt_FREE(ssGetPortInfoForOutputs(rts));
24113 }
24114 }
24115
24116 /* RTW Generated Level2 S-Function Block: '<S113>/Generated S-Function' (LagFilter_sf) */
24117 {
24118 SimStruct *rts = ssGetSFunction(S, 26);
24119 if (rts != (NULL) ) {
24120 rt_FREE(ssGetSampleTimePtr(rts));
24121 rt_FREE(ssGetOffsetTimePtr(rts));
24122 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24123
24124 /* Destroy model methods 3 */
24125 {
24126 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24127 rt_FREE(methods3);
24128 }
24129
24130 /* Destroy model methods 2 */
24131 {
24132 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24133 rt_FREE(methods2);
24134 }
24135
24136 /* Destroy state auxilliary information */
24137 {
24138 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24139 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24140 rt_FREE(periodicStatesInfo);
24141 rt_FREE(statesInfo2);
24142 }
24143
24144 {
24145 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24146 rt_FREE(ptr);
24147 }
24148
24149 {
24150 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24151 rt_FREE(ptr);
24152 }
24153
24154 {
24155 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24156 rt_FREE(ptr);
24157 }
24158
24159 {
24160 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
24161 rt_FREE(ptr);
24162 }
24163
24164 {
24165 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
24166 rt_FREE(ptr);
24167 }
24168
24169 {
24170 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
24171 rt_FREE(ptr);
24172 }
24173
24174 rt_FREE(ssGetPortInfoForInputs(rts));
24175 rt_FREE(ssGetPortInfoForOutputs(rts));
24176 rt_FREE(ssGetSFcnDWork(rts));
24177 }
24178 }
24179
24180 /* RTW Generated Level2 S-Function Block: '<S86>/Generated S-Function6' (Res180_sf) */
24181 {
24182 SimStruct *rts = ssGetSFunction(S, 27);
24183 if (rts != (NULL) ) {
24184 rt_FREE(ssGetSampleTimePtr(rts));
24185 rt_FREE(ssGetOffsetTimePtr(rts));
24186 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24187
24188 /* Destroy model methods 3 */
24189 {
24190 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24191 rt_FREE(methods3);
24192 }
24193
24194 /* Destroy model methods 2 */
24195 {
24196 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24197 rt_FREE(methods2);
24198 }
24199
24200 /* Destroy state auxilliary information */
24201 {
24202 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24203 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24204 rt_FREE(periodicStatesInfo);
24205 rt_FREE(statesInfo2);
24206 }
24207
24208 {
24209 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24210 rt_FREE(ptr);
24211 }
24212
24213 rt_FREE(ssGetPortInfoForInputs(rts));
24214 rt_FREE(ssGetPortInfoForOutputs(rts));
24215 }
24216 }
24217
24218 /* RTW Generated Level2 S-Function Block: '<S119>/Generated S-Function' (SaturationLimiter_sf) */
24219 {
24220 SimStruct *rts = ssGetSFunction(S, 28);
24221 if (rts != (NULL) ) {
24222 rt_FREE(ssGetSampleTimePtr(rts));
24223 rt_FREE(ssGetOffsetTimePtr(rts));
24224 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24225
24226 /* Destroy model methods 3 */
24227 {
24228 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24229 rt_FREE(methods3);
24230 }
24231
24232 /* Destroy model methods 2 */
24233 {
24234 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24235 rt_FREE(methods2);
24236 }
24237
24238 /* Destroy state auxilliary information */
24239 {
24240 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24241 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24242 rt_FREE(periodicStatesInfo);
24243 rt_FREE(statesInfo2);
24244 }
24245
24246 {
24247 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24248 rt_FREE(ptr);
24249 }
24250
24251 {
24252 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24253 rt_FREE(ptr);
24254 }
24255
24256 {
24257 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24258 rt_FREE(ptr);
24259 }
24260
24261 rt_FREE(ssGetPortInfoForInputs(rts));
24262 rt_FREE(ssGetPortInfoForOutputs(rts));
24263 }
24264 }
24265
24266 /* RTW Generated Level2 S-Function Block: '<S121>/Generated S-Function' (Gain_sf) */
24267 {
24268 SimStruct *rts = ssGetSFunction(S, 29);
24269 if (rts != (NULL) ) {
24270 rt_FREE(ssGetSampleTimePtr(rts));
24271 rt_FREE(ssGetOffsetTimePtr(rts));
24272 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24273
24274 /* Destroy model methods 3 */
24275 {
24276 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24277 rt_FREE(methods3);
24278 }
24279
24280 /* Destroy model methods 2 */
24281 {
24282 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24283 rt_FREE(methods2);
24284 }
24285
24286 /* Destroy state auxilliary information */
24287 {
24288 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24289 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24290 rt_FREE(periodicStatesInfo);
24291 rt_FREE(statesInfo2);
24292 }
24293
24294 {
24295 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24296 rt_FREE(ptr);
24297 }
24298
24299 {
24300 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24301 rt_FREE(ptr);
24302 }
24303
24304 rt_FREE(ssGetPortInfoForInputs(rts));
24305 rt_FREE(ssGetPortInfoForOutputs(rts));
24306 }
24307 }
24308
24309 /* RTW Generated Level2 S-Function Block: '<S118>/Generated S-Function2' (LeadLagFilter_sf) */
24310 {
24311 SimStruct *rts = ssGetSFunction(S, 30);
24312 if (rts != (NULL) ) {
24313 rt_FREE(ssGetSampleTimePtr(rts));
24314 rt_FREE(ssGetOffsetTimePtr(rts));
24315 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24316
24317 /* Destroy model methods 3 */
24318 {
24319 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24320 rt_FREE(methods3);
24321 }
24322
24323 /* Destroy model methods 2 */
24324 {
24325 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24326 rt_FREE(methods2);
24327 }
24328
24329 /* Destroy state auxilliary information */
24330 {
24331 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24332 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24333 rt_FREE(periodicStatesInfo);
24334 rt_FREE(statesInfo2);
24335 }
24336
24337 {
24338 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24339 rt_FREE(ptr);
24340 }
24341
24342 {
24343 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24344 rt_FREE(ptr);
24345 }
24346
24347 {
24348 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24349 rt_FREE(ptr);
24350 }
24351
24352 {
24353 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
24354 rt_FREE(ptr);
24355 }
24356
24357 {
24358 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
24359 rt_FREE(ptr);
24360 }
24361
24362 {
24363 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
24364 rt_FREE(ptr);
24365 }
24366
24367 {
24368 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
24369 rt_FREE(ptr);
24370 }
24371
24372 rt_FREE(ssGetPortInfoForInputs(rts));
24373 rt_FREE(ssGetPortInfoForOutputs(rts));
24374 rt_FREE(ssGetSFcnDWork(rts));
24375 }
24376 }
24377
24378 /* RTW Generated Level2 S-Function Block: '<S120>/Generated S-Function1' (SaturationLimiter_sf) */
24379 {
24380 SimStruct *rts = ssGetSFunction(S, 31);
24381 if (rts != (NULL) ) {
24382 rt_FREE(ssGetSampleTimePtr(rts));
24383 rt_FREE(ssGetOffsetTimePtr(rts));
24384 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24385
24386 /* Destroy model methods 3 */
24387 {
24388 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24389 rt_FREE(methods3);
24390 }
24391
24392 /* Destroy model methods 2 */
24393 {
24394 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24395 rt_FREE(methods2);
24396 }
24397
24398 /* Destroy state auxilliary information */
24399 {
24400 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24401 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24402 rt_FREE(periodicStatesInfo);
24403 rt_FREE(statesInfo2);
24404 }
24405
24406 {
24407 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24408 rt_FREE(ptr);
24409 }
24410
24411 {
24412 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24413 rt_FREE(ptr);
24414 }
24415
24416 {
24417 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24418 rt_FREE(ptr);
24419 }
24420
24421 rt_FREE(ssGetPortInfoForInputs(rts));
24422 rt_FREE(ssGetPortInfoForOutputs(rts));
24423 }
24424 }
24425
24426 /* RTW Generated Level2 S-Function Block: '<S117>/Generated S-Function4' (LagFilter_sf) */
24427 {
24428 SimStruct *rts = ssGetSFunction(S, 32);
24429 if (rts != (NULL) ) {
24430 rt_FREE(ssGetSampleTimePtr(rts));
24431 rt_FREE(ssGetOffsetTimePtr(rts));
24432 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24433
24434 /* Destroy model methods 3 */
24435 {
24436 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24437 rt_FREE(methods3);
24438 }
24439
24440 /* Destroy model methods 2 */
24441 {
24442 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24443 rt_FREE(methods2);
24444 }
24445
24446 /* Destroy state auxilliary information */
24447 {
24448 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24449 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24450 rt_FREE(periodicStatesInfo);
24451 rt_FREE(statesInfo2);
24452 }
24453
24454 {
24455 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24456 rt_FREE(ptr);
24457 }
24458
24459 {
24460 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24461 rt_FREE(ptr);
24462 }
24463
24464 {
24465 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24466 rt_FREE(ptr);
24467 }
24468
24469 {
24470 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
24471 rt_FREE(ptr);
24472 }
24473
24474 {
24475 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
24476 rt_FREE(ptr);
24477 }
24478
24479 {
24480 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
24481 rt_FREE(ptr);
24482 }
24483
24484 rt_FREE(ssGetPortInfoForInputs(rts));
24485 rt_FREE(ssGetPortInfoForOutputs(rts));
24486 rt_FREE(ssGetSFcnDWork(rts));
24487 }
24488 }
24489
24490 /* RTW Generated Level2 S-Function Block: '<S122>/Generated S-Function' (Gain_sf) */
24491 {
24492 SimStruct *rts = ssGetSFunction(S, 33);
24493 if (rts != (NULL) ) {
24494 rt_FREE(ssGetSampleTimePtr(rts));
24495 rt_FREE(ssGetOffsetTimePtr(rts));
24496 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24497
24498 /* Destroy model methods 3 */
24499 {
24500 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24501 rt_FREE(methods3);
24502 }
24503
24504 /* Destroy model methods 2 */
24505 {
24506 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24507 rt_FREE(methods2);
24508 }
24509
24510 /* Destroy state auxilliary information */
24511 {
24512 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24513 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24514 rt_FREE(periodicStatesInfo);
24515 rt_FREE(statesInfo2);
24516 }
24517
24518 {
24519 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24520 rt_FREE(ptr);
24521 }
24522
24523 {
24524 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24525 rt_FREE(ptr);
24526 }
24527
24528 rt_FREE(ssGetPortInfoForInputs(rts));
24529 rt_FREE(ssGetPortInfoForOutputs(rts));
24530 }
24531 }
24532
24533 /* RTW Generated Level2 S-Function Block: '<S116>/Generated S-Function4' (LagFilter_sf) */
24534 {
24535 SimStruct *rts = ssGetSFunction(S, 34);
24536 if (rts != (NULL) ) {
24537 rt_FREE(ssGetSampleTimePtr(rts));
24538 rt_FREE(ssGetOffsetTimePtr(rts));
24539 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24540
24541 /* Destroy model methods 3 */
24542 {
24543 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24544 rt_FREE(methods3);
24545 }
24546
24547 /* Destroy model methods 2 */
24548 {
24549 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24550 rt_FREE(methods2);
24551 }
24552
24553 /* Destroy state auxilliary information */
24554 {
24555 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24556 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24557 rt_FREE(periodicStatesInfo);
24558 rt_FREE(statesInfo2);
24559 }
24560
24561 {
24562 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24563 rt_FREE(ptr);
24564 }
24565
24566 {
24567 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24568 rt_FREE(ptr);
24569 }
24570
24571 {
24572 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24573 rt_FREE(ptr);
24574 }
24575
24576 {
24577 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
24578 rt_FREE(ptr);
24579 }
24580
24581 {
24582 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
24583 rt_FREE(ptr);
24584 }
24585
24586 {
24587 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
24588 rt_FREE(ptr);
24589 }
24590
24591 rt_FREE(ssGetPortInfoForInputs(rts));
24592 rt_FREE(ssGetPortInfoForOutputs(rts));
24593 rt_FREE(ssGetSFcnDWork(rts));
24594 }
24595 }
24596
24597 /* RTW Generated Level2 S-Function Block: '<S102>/Generated S-Function1' (SaturationLimiter_sf) */
24598 {
24599 SimStruct *rts = ssGetSFunction(S, 35);
24600 if (rts != (NULL) ) {
24601 rt_FREE(ssGetSampleTimePtr(rts));
24602 rt_FREE(ssGetOffsetTimePtr(rts));
24603 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24604
24605 /* Destroy model methods 3 */
24606 {
24607 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24608 rt_FREE(methods3);
24609 }
24610
24611 /* Destroy model methods 2 */
24612 {
24613 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24614 rt_FREE(methods2);
24615 }
24616
24617 /* Destroy state auxilliary information */
24618 {
24619 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24620 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24621 rt_FREE(periodicStatesInfo);
24622 rt_FREE(statesInfo2);
24623 }
24624
24625 {
24626 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24627 rt_FREE(ptr);
24628 }
24629
24630 {
24631 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24632 rt_FREE(ptr);
24633 }
24634
24635 {
24636 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24637 rt_FREE(ptr);
24638 }
24639
24640 rt_FREE(ssGetPortInfoForInputs(rts));
24641 rt_FREE(ssGetPortInfoForOutputs(rts));
24642 }
24643 }
24644
24645 /* RTW Generated Level2 S-Function Block: '<S104>/Generated S-Function' (Gain_sf) */
24646 {
24647 SimStruct *rts = ssGetSFunction(S, 36);
24648 if (rts != (NULL) ) {
24649 rt_FREE(ssGetSampleTimePtr(rts));
24650 rt_FREE(ssGetOffsetTimePtr(rts));
24651 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24652
24653 /* Destroy model methods 3 */
24654 {
24655 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24656 rt_FREE(methods3);
24657 }
24658
24659 /* Destroy model methods 2 */
24660 {
24661 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24662 rt_FREE(methods2);
24663 }
24664
24665 /* Destroy state auxilliary information */
24666 {
24667 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24668 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24669 rt_FREE(periodicStatesInfo);
24670 rt_FREE(statesInfo2);
24671 }
24672
24673 {
24674 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24675 rt_FREE(ptr);
24676 }
24677
24678 {
24679 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24680 rt_FREE(ptr);
24681 }
24682
24683 rt_FREE(ssGetPortInfoForInputs(rts));
24684 rt_FREE(ssGetPortInfoForOutputs(rts));
24685 }
24686 }
24687
24688 /* RTW Generated Level2 S-Function Block: '<S105>/Generated S-Function' (Gain_sf) */
24689 {
24690 SimStruct *rts = ssGetSFunction(S, 37);
24691 if (rts != (NULL) ) {
24692 rt_FREE(ssGetSampleTimePtr(rts));
24693 rt_FREE(ssGetOffsetTimePtr(rts));
24694 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24695
24696 /* Destroy model methods 3 */
24697 {
24698 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24699 rt_FREE(methods3);
24700 }
24701
24702 /* Destroy model methods 2 */
24703 {
24704 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24705 rt_FREE(methods2);
24706 }
24707
24708 /* Destroy state auxilliary information */
24709 {
24710 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24711 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24712 rt_FREE(periodicStatesInfo);
24713 rt_FREE(statesInfo2);
24714 }
24715
24716 {
24717 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24718 rt_FREE(ptr);
24719 }
24720
24721 {
24722 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24723 rt_FREE(ptr);
24724 }
24725
24726 rt_FREE(ssGetPortInfoForInputs(rts));
24727 rt_FREE(ssGetPortInfoForOutputs(rts));
24728 }
24729 }
24730
24731 /* RTW Generated Level2 S-Function Block: '<S100>/Generated S-Function3' (IntegratorLimited_sf) */
24732 {
24733 SimStruct *rts = ssGetSFunction(S, 38);
24734 if (rts != (NULL) ) {
24735 rt_FREE(ssGetSampleTimePtr(rts));
24736 rt_FREE(ssGetOffsetTimePtr(rts));
24737 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24738
24739 /* Destroy model methods 3 */
24740 {
24741 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24742 rt_FREE(methods3);
24743 }
24744
24745 /* Destroy model methods 2 */
24746 {
24747 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24748 rt_FREE(methods2);
24749 }
24750
24751 /* Destroy state auxilliary information */
24752 {
24753 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24754 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24755 rt_FREE(periodicStatesInfo);
24756 rt_FREE(statesInfo2);
24757 }
24758
24759 {
24760 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24761 rt_FREE(ptr);
24762 }
24763
24764 {
24765 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24766 rt_FREE(ptr);
24767 }
24768
24769 {
24770 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24771 rt_FREE(ptr);
24772 }
24773
24774 {
24775 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
24776 rt_FREE(ptr);
24777 }
24778
24779 {
24780 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
24781 rt_FREE(ptr);
24782 }
24783
24784 {
24785 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
24786 rt_FREE(ptr);
24787 }
24788
24789 {
24790 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
24791 rt_FREE(ptr);
24792 }
24793
24794 rt_FREE(ssGetPortInfoForInputs(rts));
24795 rt_FREE(ssGetPortInfoForOutputs(rts));
24796 rt_FREE(ssGetSFcnDWork(rts));
24797 }
24798 }
24799
24800 /* RTW Generated Level2 S-Function Block: '<S101>/Generated S-Function2' (LeadLagFilter_sf) */
24801 {
24802 SimStruct *rts = ssGetSFunction(S, 39);
24803 if (rts != (NULL) ) {
24804 rt_FREE(ssGetSampleTimePtr(rts));
24805 rt_FREE(ssGetOffsetTimePtr(rts));
24806 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24807
24808 /* Destroy model methods 3 */
24809 {
24810 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24811 rt_FREE(methods3);
24812 }
24813
24814 /* Destroy model methods 2 */
24815 {
24816 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24817 rt_FREE(methods2);
24818 }
24819
24820 /* Destroy state auxilliary information */
24821 {
24822 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24823 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24824 rt_FREE(periodicStatesInfo);
24825 rt_FREE(statesInfo2);
24826 }
24827
24828 {
24829 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24830 rt_FREE(ptr);
24831 }
24832
24833 {
24834 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24835 rt_FREE(ptr);
24836 }
24837
24838 {
24839 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24840 rt_FREE(ptr);
24841 }
24842
24843 {
24844 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
24845 rt_FREE(ptr);
24846 }
24847
24848 {
24849 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
24850 rt_FREE(ptr);
24851 }
24852
24853 {
24854 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
24855 rt_FREE(ptr);
24856 }
24857
24858 {
24859 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
24860 rt_FREE(ptr);
24861 }
24862
24863 rt_FREE(ssGetPortInfoForInputs(rts));
24864 rt_FREE(ssGetPortInfoForOutputs(rts));
24865 rt_FREE(ssGetSFcnDWork(rts));
24866 }
24867 }
24868
24869 /* RTW Generated Level2 S-Function Block: '<S103>/Generated S-Function1' (SaturationLimiter_sf) */
24870 {
24871 SimStruct *rts = ssGetSFunction(S, 40);
24872 if (rts != (NULL) ) {
24873 rt_FREE(ssGetSampleTimePtr(rts));
24874 rt_FREE(ssGetOffsetTimePtr(rts));
24875 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24876
24877 /* Destroy model methods 3 */
24878 {
24879 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24880 rt_FREE(methods3);
24881 }
24882
24883 /* Destroy model methods 2 */
24884 {
24885 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24886 rt_FREE(methods2);
24887 }
24888
24889 /* Destroy state auxilliary information */
24890 {
24891 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24892 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24893 rt_FREE(periodicStatesInfo);
24894 rt_FREE(statesInfo2);
24895 }
24896
24897 {
24898 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24899 rt_FREE(ptr);
24900 }
24901
24902 {
24903 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24904 rt_FREE(ptr);
24905 }
24906
24907 {
24908 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24909 rt_FREE(ptr);
24910 }
24911
24912 rt_FREE(ssGetPortInfoForInputs(rts));
24913 rt_FREE(ssGetPortInfoForOutputs(rts));
24914 }
24915 }
24916
24917 /* RTW Generated Level2 S-Function Block: '<S98>/Generated S-Function4' (LagFilter_sf) */
24918 {
24919 SimStruct *rts = ssGetSFunction(S, 41);
24920 if (rts != (NULL) ) {
24921 rt_FREE(ssGetSampleTimePtr(rts));
24922 rt_FREE(ssGetOffsetTimePtr(rts));
24923 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24924
24925 /* Destroy model methods 3 */
24926 {
24927 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24928 rt_FREE(methods3);
24929 }
24930
24931 /* Destroy model methods 2 */
24932 {
24933 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24934 rt_FREE(methods2);
24935 }
24936
24937 /* Destroy state auxilliary information */
24938 {
24939 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
24940 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
24941 rt_FREE(periodicStatesInfo);
24942 rt_FREE(statesInfo2);
24943 }
24944
24945 {
24946 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
24947 rt_FREE(ptr);
24948 }
24949
24950 {
24951 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
24952 rt_FREE(ptr);
24953 }
24954
24955 {
24956 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
24957 rt_FREE(ptr);
24958 }
24959
24960 {
24961 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
24962 rt_FREE(ptr);
24963 }
24964
24965 {
24966 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
24967 rt_FREE(ptr);
24968 }
24969
24970 {
24971 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
24972 rt_FREE(ptr);
24973 }
24974
24975 rt_FREE(ssGetPortInfoForInputs(rts));
24976 rt_FREE(ssGetPortInfoForOutputs(rts));
24977 rt_FREE(ssGetSFcnDWork(rts));
24978 }
24979 }
24980
24981 /* RTW Generated Level2 S-Function Block: '<S109>/Generated S-Function1' (SaturationLimiter_sf) */
24982 {
24983 SimStruct *rts = ssGetSFunction(S, 42);
24984 if (rts != (NULL) ) {
24985 rt_FREE(ssGetSampleTimePtr(rts));
24986 rt_FREE(ssGetOffsetTimePtr(rts));
24987 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
24988
24989 /* Destroy model methods 3 */
24990 {
24991 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
24992 rt_FREE(methods3);
24993 }
24994
24995 /* Destroy model methods 2 */
24996 {
24997 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
24998 rt_FREE(methods2);
24999 }
25000
25001 /* Destroy state auxilliary information */
25002 {
25003 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25004 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25005 rt_FREE(periodicStatesInfo);
25006 rt_FREE(statesInfo2);
25007 }
25008
25009 {
25010 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25011 rt_FREE(ptr);
25012 }
25013
25014 {
25015 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25016 rt_FREE(ptr);
25017 }
25018
25019 {
25020 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
25021 rt_FREE(ptr);
25022 }
25023
25024 rt_FREE(ssGetPortInfoForInputs(rts));
25025 rt_FREE(ssGetPortInfoForOutputs(rts));
25026 }
25027 }
25028
25029 /* RTW Generated Level2 S-Function Block: '<S111>/Generated S-Function' (Gain_sf) */
25030 {
25031 SimStruct *rts = ssGetSFunction(S, 43);
25032 if (rts != (NULL) ) {
25033 rt_FREE(ssGetSampleTimePtr(rts));
25034 rt_FREE(ssGetOffsetTimePtr(rts));
25035 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25036
25037 /* Destroy model methods 3 */
25038 {
25039 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25040 rt_FREE(methods3);
25041 }
25042
25043 /* Destroy model methods 2 */
25044 {
25045 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25046 rt_FREE(methods2);
25047 }
25048
25049 /* Destroy state auxilliary information */
25050 {
25051 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25052 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25053 rt_FREE(periodicStatesInfo);
25054 rt_FREE(statesInfo2);
25055 }
25056
25057 {
25058 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25059 rt_FREE(ptr);
25060 }
25061
25062 {
25063 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25064 rt_FREE(ptr);
25065 }
25066
25067 rt_FREE(ssGetPortInfoForInputs(rts));
25068 rt_FREE(ssGetPortInfoForOutputs(rts));
25069 }
25070 }
25071
25072 /* RTW Generated Level2 S-Function Block: '<S112>/Generated S-Function' (Gain_sf) */
25073 {
25074 SimStruct *rts = ssGetSFunction(S, 44);
25075 if (rts != (NULL) ) {
25076 rt_FREE(ssGetSampleTimePtr(rts));
25077 rt_FREE(ssGetOffsetTimePtr(rts));
25078 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25079
25080 /* Destroy model methods 3 */
25081 {
25082 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25083 rt_FREE(methods3);
25084 }
25085
25086 /* Destroy model methods 2 */
25087 {
25088 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25089 rt_FREE(methods2);
25090 }
25091
25092 /* Destroy state auxilliary information */
25093 {
25094 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25095 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25096 rt_FREE(periodicStatesInfo);
25097 rt_FREE(statesInfo2);
25098 }
25099
25100 {
25101 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25102 rt_FREE(ptr);
25103 }
25104
25105 {
25106 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25107 rt_FREE(ptr);
25108 }
25109
25110 rt_FREE(ssGetPortInfoForInputs(rts));
25111 rt_FREE(ssGetPortInfoForOutputs(rts));
25112 }
25113 }
25114
25115 /* RTW Generated Level2 S-Function Block: '<S107>/Generated S-Function3' (IntegratorLimited_sf) */
25116 {
25117 SimStruct *rts = ssGetSFunction(S, 45);
25118 if (rts != (NULL) ) {
25119 rt_FREE(ssGetSampleTimePtr(rts));
25120 rt_FREE(ssGetOffsetTimePtr(rts));
25121 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25122
25123 /* Destroy model methods 3 */
25124 {
25125 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25126 rt_FREE(methods3);
25127 }
25128
25129 /* Destroy model methods 2 */
25130 {
25131 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25132 rt_FREE(methods2);
25133 }
25134
25135 /* Destroy state auxilliary information */
25136 {
25137 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25138 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25139 rt_FREE(periodicStatesInfo);
25140 rt_FREE(statesInfo2);
25141 }
25142
25143 {
25144 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25145 rt_FREE(ptr);
25146 }
25147
25148 {
25149 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25150 rt_FREE(ptr);
25151 }
25152
25153 {
25154 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
25155 rt_FREE(ptr);
25156 }
25157
25158 {
25159 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
25160 rt_FREE(ptr);
25161 }
25162
25163 {
25164 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
25165 rt_FREE(ptr);
25166 }
25167
25168 {
25169 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
25170 rt_FREE(ptr);
25171 }
25172
25173 {
25174 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
25175 rt_FREE(ptr);
25176 }
25177
25178 rt_FREE(ssGetPortInfoForInputs(rts));
25179 rt_FREE(ssGetPortInfoForOutputs(rts));
25180 rt_FREE(ssGetSFcnDWork(rts));
25181 }
25182 }
25183
25184 /* RTW Generated Level2 S-Function Block: '<S108>/Generated S-Function2' (LeadLagFilter_sf) */
25185 {
25186 SimStruct *rts = ssGetSFunction(S, 46);
25187 if (rts != (NULL) ) {
25188 rt_FREE(ssGetSampleTimePtr(rts));
25189 rt_FREE(ssGetOffsetTimePtr(rts));
25190 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25191
25192 /* Destroy model methods 3 */
25193 {
25194 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25195 rt_FREE(methods3);
25196 }
25197
25198 /* Destroy model methods 2 */
25199 {
25200 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25201 rt_FREE(methods2);
25202 }
25203
25204 /* Destroy state auxilliary information */
25205 {
25206 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25207 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25208 rt_FREE(periodicStatesInfo);
25209 rt_FREE(statesInfo2);
25210 }
25211
25212 {
25213 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25214 rt_FREE(ptr);
25215 }
25216
25217 {
25218 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25219 rt_FREE(ptr);
25220 }
25221
25222 {
25223 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
25224 rt_FREE(ptr);
25225 }
25226
25227 {
25228 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
25229 rt_FREE(ptr);
25230 }
25231
25232 {
25233 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
25234 rt_FREE(ptr);
25235 }
25236
25237 {
25238 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
25239 rt_FREE(ptr);
25240 }
25241
25242 {
25243 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
25244 rt_FREE(ptr);
25245 }
25246
25247 rt_FREE(ssGetPortInfoForInputs(rts));
25248 rt_FREE(ssGetPortInfoForOutputs(rts));
25249 rt_FREE(ssGetSFcnDWork(rts));
25250 }
25251 }
25252
25253 /* RTW Generated Level2 S-Function Block: '<S110>/Generated S-Function1' (SaturationLimiter_sf) */
25254 {
25255 SimStruct *rts = ssGetSFunction(S, 47);
25256 if (rts != (NULL) ) {
25257 rt_FREE(ssGetSampleTimePtr(rts));
25258 rt_FREE(ssGetOffsetTimePtr(rts));
25259 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25260
25261 /* Destroy model methods 3 */
25262 {
25263 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25264 rt_FREE(methods3);
25265 }
25266
25267 /* Destroy model methods 2 */
25268 {
25269 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25270 rt_FREE(methods2);
25271 }
25272
25273 /* Destroy state auxilliary information */
25274 {
25275 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25276 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25277 rt_FREE(periodicStatesInfo);
25278 rt_FREE(statesInfo2);
25279 }
25280
25281 {
25282 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25283 rt_FREE(ptr);
25284 }
25285
25286 {
25287 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25288 rt_FREE(ptr);
25289 }
25290
25291 {
25292 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
25293 rt_FREE(ptr);
25294 }
25295
25296 rt_FREE(ssGetPortInfoForInputs(rts));
25297 rt_FREE(ssGetPortInfoForOutputs(rts));
25298 }
25299 }
25300
25301 /* RTW Generated Level2 S-Function Block: '<S106>/Generated S-Function4' (LagFilter_sf) */
25302 {
25303 SimStruct *rts = ssGetSFunction(S, 48);
25304 if (rts != (NULL) ) {
25305 rt_FREE(ssGetSampleTimePtr(rts));
25306 rt_FREE(ssGetOffsetTimePtr(rts));
25307 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25308
25309 /* Destroy model methods 3 */
25310 {
25311 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25312 rt_FREE(methods3);
25313 }
25314
25315 /* Destroy model methods 2 */
25316 {
25317 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25318 rt_FREE(methods2);
25319 }
25320
25321 /* Destroy state auxilliary information */
25322 {
25323 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25324 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25325 rt_FREE(periodicStatesInfo);
25326 rt_FREE(statesInfo2);
25327 }
25328
25329 {
25330 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25331 rt_FREE(ptr);
25332 }
25333
25334 {
25335 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25336 rt_FREE(ptr);
25337 }
25338
25339 {
25340 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
25341 rt_FREE(ptr);
25342 }
25343
25344 {
25345 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
25346 rt_FREE(ptr);
25347 }
25348
25349 {
25350 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
25351 rt_FREE(ptr);
25352 }
25353
25354 {
25355 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
25356 rt_FREE(ptr);
25357 }
25358
25359 rt_FREE(ssGetPortInfoForInputs(rts));
25360 rt_FREE(ssGetPortInfoForOutputs(rts));
25361 rt_FREE(ssGetSFcnDWork(rts));
25362 }
25363 }
25364
25365 /* RTW Generated Level2 S-Function Block: '<S81>/Generated S-Function5' (Fader_sf) */
25366 {
25367 SimStruct *rts = ssGetSFunction(S, 49);
25368 if (rts != (NULL) ) {
25369 rt_FREE(ssGetSampleTimePtr(rts));
25370 rt_FREE(ssGetOffsetTimePtr(rts));
25371 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25372
25373 /* Destroy model methods 3 */
25374 {
25375 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25376 rt_FREE(methods3);
25377 }
25378
25379 /* Destroy model methods 2 */
25380 {
25381 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25382 rt_FREE(methods2);
25383 }
25384
25385 /* Destroy state auxilliary information */
25386 {
25387 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25388 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25389 rt_FREE(periodicStatesInfo);
25390 rt_FREE(statesInfo2);
25391 }
25392
25393 {
25394 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25395 rt_FREE(ptr);
25396 }
25397
25398 {
25399 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25400 rt_FREE(ptr);
25401 }
25402
25403 {
25404 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
25405 rt_FREE(ptr);
25406 }
25407
25408 {
25409 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
25410 rt_FREE(ptr);
25411 }
25412
25413 {
25414 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
25415 rt_FREE(ptr);
25416 }
25417
25418 {
25419 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
25420 rt_FREE(ptr);
25421 }
25422
25423 rt_FREE(ssGetPortInfoForInputs(rts));
25424 rt_FREE(ssGetPortInfoForOutputs(rts));
25425 rt_FREE(ssGetSFcnDWork(rts));
25426 }
25427 }
25428
25429 /* RTW Generated Level2 S-Function Block: '<S130>/Generated S-Function4' (LagFilter_sf) */
25430 {
25431 SimStruct *rts = ssGetSFunction(S, 50);
25432 if (rts != (NULL) ) {
25433 rt_FREE(ssGetSampleTimePtr(rts));
25434 rt_FREE(ssGetOffsetTimePtr(rts));
25435 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25436
25437 /* Destroy model methods 3 */
25438 {
25439 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25440 rt_FREE(methods3);
25441 }
25442
25443 /* Destroy model methods 2 */
25444 {
25445 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25446 rt_FREE(methods2);
25447 }
25448
25449 /* Destroy state auxilliary information */
25450 {
25451 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25452 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25453 rt_FREE(periodicStatesInfo);
25454 rt_FREE(statesInfo2);
25455 }
25456
25457 {
25458 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25459 rt_FREE(ptr);
25460 }
25461
25462 {
25463 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25464 rt_FREE(ptr);
25465 }
25466
25467 {
25468 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
25469 rt_FREE(ptr);
25470 }
25471
25472 {
25473 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
25474 rt_FREE(ptr);
25475 }
25476
25477 {
25478 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
25479 rt_FREE(ptr);
25480 }
25481
25482 {
25483 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
25484 rt_FREE(ptr);
25485 }
25486
25487 rt_FREE(ssGetPortInfoForInputs(rts));
25488 rt_FREE(ssGetPortInfoForOutputs(rts));
25489 rt_FREE(ssGetSFcnDWork(rts));
25490 }
25491 }
25492
25493 /* RTW Generated Level2 S-Function Block: '<S125>/Generated S-Function6' (Res180_sf) */
25494 {
25495 SimStruct *rts = ssGetSFunction(S, 51);
25496 if (rts != (NULL) ) {
25497 rt_FREE(ssGetSampleTimePtr(rts));
25498 rt_FREE(ssGetOffsetTimePtr(rts));
25499 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25500
25501 /* Destroy model methods 3 */
25502 {
25503 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25504 rt_FREE(methods3);
25505 }
25506
25507 /* Destroy model methods 2 */
25508 {
25509 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25510 rt_FREE(methods2);
25511 }
25512
25513 /* Destroy state auxilliary information */
25514 {
25515 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25516 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25517 rt_FREE(periodicStatesInfo);
25518 rt_FREE(statesInfo2);
25519 }
25520
25521 {
25522 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25523 rt_FREE(ptr);
25524 }
25525
25526 rt_FREE(ssGetPortInfoForInputs(rts));
25527 rt_FREE(ssGetPortInfoForOutputs(rts));
25528 }
25529 }
25530
25531 /* RTW Generated Level2 S-Function Block: '<S134>/Generated S-Function1' (SaturationLimiter_sf) */
25532 {
25533 SimStruct *rts = ssGetSFunction(S, 52);
25534 if (rts != (NULL) ) {
25535 rt_FREE(ssGetSampleTimePtr(rts));
25536 rt_FREE(ssGetOffsetTimePtr(rts));
25537 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25538
25539 /* Destroy model methods 3 */
25540 {
25541 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25542 rt_FREE(methods3);
25543 }
25544
25545 /* Destroy model methods 2 */
25546 {
25547 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25548 rt_FREE(methods2);
25549 }
25550
25551 /* Destroy state auxilliary information */
25552 {
25553 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25554 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25555 rt_FREE(periodicStatesInfo);
25556 rt_FREE(statesInfo2);
25557 }
25558
25559 {
25560 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25561 rt_FREE(ptr);
25562 }
25563
25564 {
25565 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25566 rt_FREE(ptr);
25567 }
25568
25569 {
25570 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
25571 rt_FREE(ptr);
25572 }
25573
25574 rt_FREE(ssGetPortInfoForInputs(rts));
25575 rt_FREE(ssGetPortInfoForOutputs(rts));
25576 }
25577 }
25578
25579 /* RTW Generated Level2 S-Function Block: '<S135>/Generated S-Function' (Gain_sf) */
25580 {
25581 SimStruct *rts = ssGetSFunction(S, 53);
25582 if (rts != (NULL) ) {
25583 rt_FREE(ssGetSampleTimePtr(rts));
25584 rt_FREE(ssGetOffsetTimePtr(rts));
25585 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25586
25587 /* Destroy model methods 3 */
25588 {
25589 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25590 rt_FREE(methods3);
25591 }
25592
25593 /* Destroy model methods 2 */
25594 {
25595 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25596 rt_FREE(methods2);
25597 }
25598
25599 /* Destroy state auxilliary information */
25600 {
25601 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25602 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25603 rt_FREE(periodicStatesInfo);
25604 rt_FREE(statesInfo2);
25605 }
25606
25607 {
25608 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25609 rt_FREE(ptr);
25610 }
25611
25612 {
25613 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25614 rt_FREE(ptr);
25615 }
25616
25617 rt_FREE(ssGetPortInfoForInputs(rts));
25618 rt_FREE(ssGetPortInfoForOutputs(rts));
25619 }
25620 }
25621
25622 /* RTW Generated Level2 S-Function Block: '<S136>/Generated S-Function' (Gain_sf) */
25623 {
25624 SimStruct *rts = ssGetSFunction(S, 54);
25625 if (rts != (NULL) ) {
25626 rt_FREE(ssGetSampleTimePtr(rts));
25627 rt_FREE(ssGetOffsetTimePtr(rts));
25628 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25629
25630 /* Destroy model methods 3 */
25631 {
25632 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25633 rt_FREE(methods3);
25634 }
25635
25636 /* Destroy model methods 2 */
25637 {
25638 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25639 rt_FREE(methods2);
25640 }
25641
25642 /* Destroy state auxilliary information */
25643 {
25644 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25645 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25646 rt_FREE(periodicStatesInfo);
25647 rt_FREE(statesInfo2);
25648 }
25649
25650 {
25651 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25652 rt_FREE(ptr);
25653 }
25654
25655 {
25656 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25657 rt_FREE(ptr);
25658 }
25659
25660 rt_FREE(ssGetPortInfoForInputs(rts));
25661 rt_FREE(ssGetPortInfoForOutputs(rts));
25662 }
25663 }
25664
25665 /* RTW Generated Level2 S-Function Block: '<S137>/Generated S-Function' (Gain_sf) */
25666 {
25667 SimStruct *rts = ssGetSFunction(S, 55);
25668 if (rts != (NULL) ) {
25669 rt_FREE(ssGetSampleTimePtr(rts));
25670 rt_FREE(ssGetOffsetTimePtr(rts));
25671 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25672
25673 /* Destroy model methods 3 */
25674 {
25675 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25676 rt_FREE(methods3);
25677 }
25678
25679 /* Destroy model methods 2 */
25680 {
25681 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25682 rt_FREE(methods2);
25683 }
25684
25685 /* Destroy state auxilliary information */
25686 {
25687 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25688 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25689 rt_FREE(periodicStatesInfo);
25690 rt_FREE(statesInfo2);
25691 }
25692
25693 {
25694 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25695 rt_FREE(ptr);
25696 }
25697
25698 {
25699 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25700 rt_FREE(ptr);
25701 }
25702
25703 rt_FREE(ssGetPortInfoForInputs(rts));
25704 rt_FREE(ssGetPortInfoForOutputs(rts));
25705 }
25706 }
25707
25708 /* RTW Generated Level2 S-Function Block: '<S132>/Generated S-Function3' (IntegratorLimited_sf) */
25709 {
25710 SimStruct *rts = ssGetSFunction(S, 56);
25711 if (rts != (NULL) ) {
25712 rt_FREE(ssGetSampleTimePtr(rts));
25713 rt_FREE(ssGetOffsetTimePtr(rts));
25714 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25715
25716 /* Destroy model methods 3 */
25717 {
25718 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25719 rt_FREE(methods3);
25720 }
25721
25722 /* Destroy model methods 2 */
25723 {
25724 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25725 rt_FREE(methods2);
25726 }
25727
25728 /* Destroy state auxilliary information */
25729 {
25730 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25731 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25732 rt_FREE(periodicStatesInfo);
25733 rt_FREE(statesInfo2);
25734 }
25735
25736 {
25737 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25738 rt_FREE(ptr);
25739 }
25740
25741 {
25742 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25743 rt_FREE(ptr);
25744 }
25745
25746 {
25747 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
25748 rt_FREE(ptr);
25749 }
25750
25751 {
25752 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
25753 rt_FREE(ptr);
25754 }
25755
25756 {
25757 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
25758 rt_FREE(ptr);
25759 }
25760
25761 {
25762 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
25763 rt_FREE(ptr);
25764 }
25765
25766 {
25767 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
25768 rt_FREE(ptr);
25769 }
25770
25771 rt_FREE(ssGetPortInfoForInputs(rts));
25772 rt_FREE(ssGetPortInfoForOutputs(rts));
25773 rt_FREE(ssGetSFcnDWork(rts));
25774 }
25775 }
25776
25777 /* RTW Generated Level2 S-Function Block: '<S133>/Generated S-Function2' (LeadLagFilter_sf) */
25778 {
25779 SimStruct *rts = ssGetSFunction(S, 57);
25780 if (rts != (NULL) ) {
25781 rt_FREE(ssGetSampleTimePtr(rts));
25782 rt_FREE(ssGetOffsetTimePtr(rts));
25783 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25784
25785 /* Destroy model methods 3 */
25786 {
25787 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25788 rt_FREE(methods3);
25789 }
25790
25791 /* Destroy model methods 2 */
25792 {
25793 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25794 rt_FREE(methods2);
25795 }
25796
25797 /* Destroy state auxilliary information */
25798 {
25799 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25800 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25801 rt_FREE(periodicStatesInfo);
25802 rt_FREE(statesInfo2);
25803 }
25804
25805 {
25806 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25807 rt_FREE(ptr);
25808 }
25809
25810 {
25811 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25812 rt_FREE(ptr);
25813 }
25814
25815 {
25816 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
25817 rt_FREE(ptr);
25818 }
25819
25820 {
25821 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
25822 rt_FREE(ptr);
25823 }
25824
25825 {
25826 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
25827 rt_FREE(ptr);
25828 }
25829
25830 {
25831 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
25832 rt_FREE(ptr);
25833 }
25834
25835 {
25836 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
25837 rt_FREE(ptr);
25838 }
25839
25840 rt_FREE(ssGetPortInfoForInputs(rts));
25841 rt_FREE(ssGetPortInfoForOutputs(rts));
25842 rt_FREE(ssGetSFcnDWork(rts));
25843 }
25844 }
25845
25846 /* RTW Generated Level2 S-Function Block: '<S131>/Generated S-Function4' (LagFilter_sf) */
25847 {
25848 SimStruct *rts = ssGetSFunction(S, 58);
25849 if (rts != (NULL) ) {
25850 rt_FREE(ssGetSampleTimePtr(rts));
25851 rt_FREE(ssGetOffsetTimePtr(rts));
25852 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25853
25854 /* Destroy model methods 3 */
25855 {
25856 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25857 rt_FREE(methods3);
25858 }
25859
25860 /* Destroy model methods 2 */
25861 {
25862 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25863 rt_FREE(methods2);
25864 }
25865
25866 /* Destroy state auxilliary information */
25867 {
25868 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25869 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25870 rt_FREE(periodicStatesInfo);
25871 rt_FREE(statesInfo2);
25872 }
25873
25874 {
25875 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25876 rt_FREE(ptr);
25877 }
25878
25879 {
25880 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25881 rt_FREE(ptr);
25882 }
25883
25884 {
25885 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
25886 rt_FREE(ptr);
25887 }
25888
25889 {
25890 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
25891 rt_FREE(ptr);
25892 }
25893
25894 {
25895 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
25896 rt_FREE(ptr);
25897 }
25898
25899 {
25900 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
25901 rt_FREE(ptr);
25902 }
25903
25904 rt_FREE(ssGetPortInfoForInputs(rts));
25905 rt_FREE(ssGetPortInfoForOutputs(rts));
25906 rt_FREE(ssGetSFcnDWork(rts));
25907 }
25908 }
25909
25910 /* RTW Generated Level2 S-Function Block: '<S155>/Generated S-Function1' (SaturationLimiter_sf) */
25911 {
25912 SimStruct *rts = ssGetSFunction(S, 59);
25913 if (rts != (NULL) ) {
25914 rt_FREE(ssGetSampleTimePtr(rts));
25915 rt_FREE(ssGetOffsetTimePtr(rts));
25916 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25917
25918 /* Destroy model methods 3 */
25919 {
25920 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25921 rt_FREE(methods3);
25922 }
25923
25924 /* Destroy model methods 2 */
25925 {
25926 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25927 rt_FREE(methods2);
25928 }
25929
25930 /* Destroy state auxilliary information */
25931 {
25932 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25933 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25934 rt_FREE(periodicStatesInfo);
25935 rt_FREE(statesInfo2);
25936 }
25937
25938 {
25939 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25940 rt_FREE(ptr);
25941 }
25942
25943 {
25944 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
25945 rt_FREE(ptr);
25946 }
25947
25948 {
25949 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
25950 rt_FREE(ptr);
25951 }
25952
25953 rt_FREE(ssGetPortInfoForInputs(rts));
25954 rt_FREE(ssGetPortInfoForOutputs(rts));
25955 }
25956 }
25957
25958 /* RTW Generated Level2 S-Function Block: '<S128>/Generated S-Function6' (Res180_sf) */
25959 {
25960 SimStruct *rts = ssGetSFunction(S, 60);
25961 if (rts != (NULL) ) {
25962 rt_FREE(ssGetSampleTimePtr(rts));
25963 rt_FREE(ssGetOffsetTimePtr(rts));
25964 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
25965
25966 /* Destroy model methods 3 */
25967 {
25968 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
25969 rt_FREE(methods3);
25970 }
25971
25972 /* Destroy model methods 2 */
25973 {
25974 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
25975 rt_FREE(methods2);
25976 }
25977
25978 /* Destroy state auxilliary information */
25979 {
25980 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
25981 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
25982 rt_FREE(periodicStatesInfo);
25983 rt_FREE(statesInfo2);
25984 }
25985
25986 {
25987 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
25988 rt_FREE(ptr);
25989 }
25990
25991 rt_FREE(ssGetPortInfoForInputs(rts));
25992 rt_FREE(ssGetPortInfoForOutputs(rts));
25993 }
25994 }
25995
25996 /* RTW Generated Level2 S-Function Block: '<S154>/Generated S-Function1' (SaturationLimiter_sf) */
25997 {
25998 SimStruct *rts = ssGetSFunction(S, 61);
25999 if (rts != (NULL) ) {
26000 rt_FREE(ssGetSampleTimePtr(rts));
26001 rt_FREE(ssGetOffsetTimePtr(rts));
26002 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26003
26004 /* Destroy model methods 3 */
26005 {
26006 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26007 rt_FREE(methods3);
26008 }
26009
26010 /* Destroy model methods 2 */
26011 {
26012 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26013 rt_FREE(methods2);
26014 }
26015
26016 /* Destroy state auxilliary information */
26017 {
26018 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26019 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26020 rt_FREE(periodicStatesInfo);
26021 rt_FREE(statesInfo2);
26022 }
26023
26024 {
26025 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26026 rt_FREE(ptr);
26027 }
26028
26029 {
26030 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26031 rt_FREE(ptr);
26032 }
26033
26034 {
26035 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
26036 rt_FREE(ptr);
26037 }
26038
26039 rt_FREE(ssGetPortInfoForInputs(rts));
26040 rt_FREE(ssGetPortInfoForOutputs(rts));
26041 }
26042 }
26043
26044 /* RTW Generated Level2 S-Function Block: '<S158>/Generated S-Function' (Gain_sf) */
26045 {
26046 SimStruct *rts = ssGetSFunction(S, 62);
26047 if (rts != (NULL) ) {
26048 rt_FREE(ssGetSampleTimePtr(rts));
26049 rt_FREE(ssGetOffsetTimePtr(rts));
26050 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26051
26052 /* Destroy model methods 3 */
26053 {
26054 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26055 rt_FREE(methods3);
26056 }
26057
26058 /* Destroy model methods 2 */
26059 {
26060 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26061 rt_FREE(methods2);
26062 }
26063
26064 /* Destroy state auxilliary information */
26065 {
26066 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26067 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26068 rt_FREE(periodicStatesInfo);
26069 rt_FREE(statesInfo2);
26070 }
26071
26072 {
26073 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26074 rt_FREE(ptr);
26075 }
26076
26077 {
26078 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26079 rt_FREE(ptr);
26080 }
26081
26082 rt_FREE(ssGetPortInfoForInputs(rts));
26083 rt_FREE(ssGetPortInfoForOutputs(rts));
26084 }
26085 }
26086
26087 /* RTW Generated Level2 S-Function Block: '<S152>/Generated S-Function2' (LeadLagFilter_sf) */
26088 {
26089 SimStruct *rts = ssGetSFunction(S, 63);
26090 if (rts != (NULL) ) {
26091 rt_FREE(ssGetSampleTimePtr(rts));
26092 rt_FREE(ssGetOffsetTimePtr(rts));
26093 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26094
26095 /* Destroy model methods 3 */
26096 {
26097 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26098 rt_FREE(methods3);
26099 }
26100
26101 /* Destroy model methods 2 */
26102 {
26103 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26104 rt_FREE(methods2);
26105 }
26106
26107 /* Destroy state auxilliary information */
26108 {
26109 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26110 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26111 rt_FREE(periodicStatesInfo);
26112 rt_FREE(statesInfo2);
26113 }
26114
26115 {
26116 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26117 rt_FREE(ptr);
26118 }
26119
26120 {
26121 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26122 rt_FREE(ptr);
26123 }
26124
26125 {
26126 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
26127 rt_FREE(ptr);
26128 }
26129
26130 {
26131 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
26132 rt_FREE(ptr);
26133 }
26134
26135 {
26136 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
26137 rt_FREE(ptr);
26138 }
26139
26140 {
26141 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
26142 rt_FREE(ptr);
26143 }
26144
26145 {
26146 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
26147 rt_FREE(ptr);
26148 }
26149
26150 rt_FREE(ssGetPortInfoForInputs(rts));
26151 rt_FREE(ssGetPortInfoForOutputs(rts));
26152 rt_FREE(ssGetSFcnDWork(rts));
26153 }
26154 }
26155
26156 /* RTW Generated Level2 S-Function Block: '<S128>/Generated S-Function1' (Res180_sf) */
26157 {
26158 SimStruct *rts = ssGetSFunction(S, 64);
26159 if (rts != (NULL) ) {
26160 rt_FREE(ssGetSampleTimePtr(rts));
26161 rt_FREE(ssGetOffsetTimePtr(rts));
26162 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26163
26164 /* Destroy model methods 3 */
26165 {
26166 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26167 rt_FREE(methods3);
26168 }
26169
26170 /* Destroy model methods 2 */
26171 {
26172 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26173 rt_FREE(methods2);
26174 }
26175
26176 /* Destroy state auxilliary information */
26177 {
26178 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26179 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26180 rt_FREE(periodicStatesInfo);
26181 rt_FREE(statesInfo2);
26182 }
26183
26184 {
26185 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26186 rt_FREE(ptr);
26187 }
26188
26189 rt_FREE(ssGetPortInfoForInputs(rts));
26190 rt_FREE(ssGetPortInfoForOutputs(rts));
26191 }
26192 }
26193
26194 /* RTW Generated Level2 S-Function Block: '<S153>/Generated S-Function1' (SaturationLimiter_sf) */
26195 {
26196 SimStruct *rts = ssGetSFunction(S, 65);
26197 if (rts != (NULL) ) {
26198 rt_FREE(ssGetSampleTimePtr(rts));
26199 rt_FREE(ssGetOffsetTimePtr(rts));
26200 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26201
26202 /* Destroy model methods 3 */
26203 {
26204 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26205 rt_FREE(methods3);
26206 }
26207
26208 /* Destroy model methods 2 */
26209 {
26210 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26211 rt_FREE(methods2);
26212 }
26213
26214 /* Destroy state auxilliary information */
26215 {
26216 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26217 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26218 rt_FREE(periodicStatesInfo);
26219 rt_FREE(statesInfo2);
26220 }
26221
26222 {
26223 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26224 rt_FREE(ptr);
26225 }
26226
26227 {
26228 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26229 rt_FREE(ptr);
26230 }
26231
26232 {
26233 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
26234 rt_FREE(ptr);
26235 }
26236
26237 rt_FREE(ssGetPortInfoForInputs(rts));
26238 rt_FREE(ssGetPortInfoForOutputs(rts));
26239 }
26240 }
26241
26242 /* RTW Generated Level2 S-Function Block: '<S156>/Generated S-Function' (Gain_sf) */
26243 {
26244 SimStruct *rts = ssGetSFunction(S, 66);
26245 if (rts != (NULL) ) {
26246 rt_FREE(ssGetSampleTimePtr(rts));
26247 rt_FREE(ssGetOffsetTimePtr(rts));
26248 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26249
26250 /* Destroy model methods 3 */
26251 {
26252 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26253 rt_FREE(methods3);
26254 }
26255
26256 /* Destroy model methods 2 */
26257 {
26258 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26259 rt_FREE(methods2);
26260 }
26261
26262 /* Destroy state auxilliary information */
26263 {
26264 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26265 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26266 rt_FREE(periodicStatesInfo);
26267 rt_FREE(statesInfo2);
26268 }
26269
26270 {
26271 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26272 rt_FREE(ptr);
26273 }
26274
26275 {
26276 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26277 rt_FREE(ptr);
26278 }
26279
26280 rt_FREE(ssGetPortInfoForInputs(rts));
26281 rt_FREE(ssGetPortInfoForOutputs(rts));
26282 }
26283 }
26284
26285 /* RTW Generated Level2 S-Function Block: '<S157>/Generated S-Function' (Gain_sf) */
26286 {
26287 SimStruct *rts = ssGetSFunction(S, 67);
26288 if (rts != (NULL) ) {
26289 rt_FREE(ssGetSampleTimePtr(rts));
26290 rt_FREE(ssGetOffsetTimePtr(rts));
26291 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26292
26293 /* Destroy model methods 3 */
26294 {
26295 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26296 rt_FREE(methods3);
26297 }
26298
26299 /* Destroy model methods 2 */
26300 {
26301 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26302 rt_FREE(methods2);
26303 }
26304
26305 /* Destroy state auxilliary information */
26306 {
26307 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26308 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26309 rt_FREE(periodicStatesInfo);
26310 rt_FREE(statesInfo2);
26311 }
26312
26313 {
26314 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26315 rt_FREE(ptr);
26316 }
26317
26318 {
26319 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26320 rt_FREE(ptr);
26321 }
26322
26323 rt_FREE(ssGetPortInfoForInputs(rts));
26324 rt_FREE(ssGetPortInfoForOutputs(rts));
26325 }
26326 }
26327
26328 /* RTW Generated Level2 S-Function Block: '<S150>/Generated S-Function3' (IntegratorLimited_sf) */
26329 {
26330 SimStruct *rts = ssGetSFunction(S, 68);
26331 if (rts != (NULL) ) {
26332 rt_FREE(ssGetSampleTimePtr(rts));
26333 rt_FREE(ssGetOffsetTimePtr(rts));
26334 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26335
26336 /* Destroy model methods 3 */
26337 {
26338 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26339 rt_FREE(methods3);
26340 }
26341
26342 /* Destroy model methods 2 */
26343 {
26344 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26345 rt_FREE(methods2);
26346 }
26347
26348 /* Destroy state auxilliary information */
26349 {
26350 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26351 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26352 rt_FREE(periodicStatesInfo);
26353 rt_FREE(statesInfo2);
26354 }
26355
26356 {
26357 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26358 rt_FREE(ptr);
26359 }
26360
26361 {
26362 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26363 rt_FREE(ptr);
26364 }
26365
26366 {
26367 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
26368 rt_FREE(ptr);
26369 }
26370
26371 {
26372 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
26373 rt_FREE(ptr);
26374 }
26375
26376 {
26377 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
26378 rt_FREE(ptr);
26379 }
26380
26381 {
26382 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
26383 rt_FREE(ptr);
26384 }
26385
26386 {
26387 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
26388 rt_FREE(ptr);
26389 }
26390
26391 rt_FREE(ssGetPortInfoForInputs(rts));
26392 rt_FREE(ssGetPortInfoForOutputs(rts));
26393 rt_FREE(ssGetSFcnDWork(rts));
26394 }
26395 }
26396
26397 /* RTW Generated Level2 S-Function Block: '<S151>/Generated S-Function2' (LeadLagFilter_sf) */
26398 {
26399 SimStruct *rts = ssGetSFunction(S, 69);
26400 if (rts != (NULL) ) {
26401 rt_FREE(ssGetSampleTimePtr(rts));
26402 rt_FREE(ssGetOffsetTimePtr(rts));
26403 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26404
26405 /* Destroy model methods 3 */
26406 {
26407 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26408 rt_FREE(methods3);
26409 }
26410
26411 /* Destroy model methods 2 */
26412 {
26413 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26414 rt_FREE(methods2);
26415 }
26416
26417 /* Destroy state auxilliary information */
26418 {
26419 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26420 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26421 rt_FREE(periodicStatesInfo);
26422 rt_FREE(statesInfo2);
26423 }
26424
26425 {
26426 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26427 rt_FREE(ptr);
26428 }
26429
26430 {
26431 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26432 rt_FREE(ptr);
26433 }
26434
26435 {
26436 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
26437 rt_FREE(ptr);
26438 }
26439
26440 {
26441 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
26442 rt_FREE(ptr);
26443 }
26444
26445 {
26446 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
26447 rt_FREE(ptr);
26448 }
26449
26450 {
26451 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
26452 rt_FREE(ptr);
26453 }
26454
26455 {
26456 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
26457 rt_FREE(ptr);
26458 }
26459
26460 rt_FREE(ssGetPortInfoForInputs(rts));
26461 rt_FREE(ssGetPortInfoForOutputs(rts));
26462 rt_FREE(ssGetSFcnDWork(rts));
26463 }
26464 }
26465
26466 /* RTW Generated Level2 S-Function Block: '<S149>/Generated S-Function4' (LagFilter_sf) */
26467 {
26468 SimStruct *rts = ssGetSFunction(S, 70);
26469 if (rts != (NULL) ) {
26470 rt_FREE(ssGetSampleTimePtr(rts));
26471 rt_FREE(ssGetOffsetTimePtr(rts));
26472 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26473
26474 /* Destroy model methods 3 */
26475 {
26476 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26477 rt_FREE(methods3);
26478 }
26479
26480 /* Destroy model methods 2 */
26481 {
26482 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26483 rt_FREE(methods2);
26484 }
26485
26486 /* Destroy state auxilliary information */
26487 {
26488 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26489 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26490 rt_FREE(periodicStatesInfo);
26491 rt_FREE(statesInfo2);
26492 }
26493
26494 {
26495 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26496 rt_FREE(ptr);
26497 }
26498
26499 {
26500 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26501 rt_FREE(ptr);
26502 }
26503
26504 {
26505 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
26506 rt_FREE(ptr);
26507 }
26508
26509 {
26510 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
26511 rt_FREE(ptr);
26512 }
26513
26514 {
26515 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
26516 rt_FREE(ptr);
26517 }
26518
26519 {
26520 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
26521 rt_FREE(ptr);
26522 }
26523
26524 rt_FREE(ssGetPortInfoForInputs(rts));
26525 rt_FREE(ssGetPortInfoForOutputs(rts));
26526 rt_FREE(ssGetSFcnDWork(rts));
26527 }
26528 }
26529
26530 /* RTW Generated Level2 S-Function Block: '<S145>/Generated S-Function1' (SaturationLimiter_sf) */
26531 {
26532 SimStruct *rts = ssGetSFunction(S, 71);
26533 if (rts != (NULL) ) {
26534 rt_FREE(ssGetSampleTimePtr(rts));
26535 rt_FREE(ssGetOffsetTimePtr(rts));
26536 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26537
26538 /* Destroy model methods 3 */
26539 {
26540 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26541 rt_FREE(methods3);
26542 }
26543
26544 /* Destroy model methods 2 */
26545 {
26546 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26547 rt_FREE(methods2);
26548 }
26549
26550 /* Destroy state auxilliary information */
26551 {
26552 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26553 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26554 rt_FREE(periodicStatesInfo);
26555 rt_FREE(statesInfo2);
26556 }
26557
26558 {
26559 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26560 rt_FREE(ptr);
26561 }
26562
26563 {
26564 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26565 rt_FREE(ptr);
26566 }
26567
26568 {
26569 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
26570 rt_FREE(ptr);
26571 }
26572
26573 rt_FREE(ssGetPortInfoForInputs(rts));
26574 rt_FREE(ssGetPortInfoForOutputs(rts));
26575 }
26576 }
26577
26578 /* RTW Generated Level2 S-Function Block: '<S127>/Generated S-Function6' (Res180_sf) */
26579 {
26580 SimStruct *rts = ssGetSFunction(S, 72);
26581 if (rts != (NULL) ) {
26582 rt_FREE(ssGetSampleTimePtr(rts));
26583 rt_FREE(ssGetOffsetTimePtr(rts));
26584 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26585
26586 /* Destroy model methods 3 */
26587 {
26588 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26589 rt_FREE(methods3);
26590 }
26591
26592 /* Destroy model methods 2 */
26593 {
26594 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26595 rt_FREE(methods2);
26596 }
26597
26598 /* Destroy state auxilliary information */
26599 {
26600 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26601 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26602 rt_FREE(periodicStatesInfo);
26603 rt_FREE(statesInfo2);
26604 }
26605
26606 {
26607 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26608 rt_FREE(ptr);
26609 }
26610
26611 rt_FREE(ssGetPortInfoForInputs(rts));
26612 rt_FREE(ssGetPortInfoForOutputs(rts));
26613 }
26614 }
26615
26616 /* RTW Generated Level2 S-Function Block: '<S144>/Generated S-Function1' (SaturationLimiter_sf) */
26617 {
26618 SimStruct *rts = ssGetSFunction(S, 73);
26619 if (rts != (NULL) ) {
26620 rt_FREE(ssGetSampleTimePtr(rts));
26621 rt_FREE(ssGetOffsetTimePtr(rts));
26622 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26623
26624 /* Destroy model methods 3 */
26625 {
26626 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26627 rt_FREE(methods3);
26628 }
26629
26630 /* Destroy model methods 2 */
26631 {
26632 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26633 rt_FREE(methods2);
26634 }
26635
26636 /* Destroy state auxilliary information */
26637 {
26638 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26639 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26640 rt_FREE(periodicStatesInfo);
26641 rt_FREE(statesInfo2);
26642 }
26643
26644 {
26645 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26646 rt_FREE(ptr);
26647 }
26648
26649 {
26650 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26651 rt_FREE(ptr);
26652 }
26653
26654 {
26655 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
26656 rt_FREE(ptr);
26657 }
26658
26659 rt_FREE(ssGetPortInfoForInputs(rts));
26660 rt_FREE(ssGetPortInfoForOutputs(rts));
26661 }
26662 }
26663
26664 /* RTW Generated Level2 S-Function Block: '<S148>/Generated S-Function' (Gain_sf) */
26665 {
26666 SimStruct *rts = ssGetSFunction(S, 74);
26667 if (rts != (NULL) ) {
26668 rt_FREE(ssGetSampleTimePtr(rts));
26669 rt_FREE(ssGetOffsetTimePtr(rts));
26670 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26671
26672 /* Destroy model methods 3 */
26673 {
26674 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26675 rt_FREE(methods3);
26676 }
26677
26678 /* Destroy model methods 2 */
26679 {
26680 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26681 rt_FREE(methods2);
26682 }
26683
26684 /* Destroy state auxilliary information */
26685 {
26686 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26687 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26688 rt_FREE(periodicStatesInfo);
26689 rt_FREE(statesInfo2);
26690 }
26691
26692 {
26693 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26694 rt_FREE(ptr);
26695 }
26696
26697 {
26698 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26699 rt_FREE(ptr);
26700 }
26701
26702 rt_FREE(ssGetPortInfoForInputs(rts));
26703 rt_FREE(ssGetPortInfoForOutputs(rts));
26704 }
26705 }
26706
26707 /* RTW Generated Level2 S-Function Block: '<S142>/Generated S-Function2' (LeadLagFilter_sf) */
26708 {
26709 SimStruct *rts = ssGetSFunction(S, 75);
26710 if (rts != (NULL) ) {
26711 rt_FREE(ssGetSampleTimePtr(rts));
26712 rt_FREE(ssGetOffsetTimePtr(rts));
26713 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26714
26715 /* Destroy model methods 3 */
26716 {
26717 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26718 rt_FREE(methods3);
26719 }
26720
26721 /* Destroy model methods 2 */
26722 {
26723 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26724 rt_FREE(methods2);
26725 }
26726
26727 /* Destroy state auxilliary information */
26728 {
26729 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26730 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26731 rt_FREE(periodicStatesInfo);
26732 rt_FREE(statesInfo2);
26733 }
26734
26735 {
26736 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26737 rt_FREE(ptr);
26738 }
26739
26740 {
26741 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26742 rt_FREE(ptr);
26743 }
26744
26745 {
26746 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
26747 rt_FREE(ptr);
26748 }
26749
26750 {
26751 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
26752 rt_FREE(ptr);
26753 }
26754
26755 {
26756 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
26757 rt_FREE(ptr);
26758 }
26759
26760 {
26761 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
26762 rt_FREE(ptr);
26763 }
26764
26765 {
26766 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
26767 rt_FREE(ptr);
26768 }
26769
26770 rt_FREE(ssGetPortInfoForInputs(rts));
26771 rt_FREE(ssGetPortInfoForOutputs(rts));
26772 rt_FREE(ssGetSFcnDWork(rts));
26773 }
26774 }
26775
26776 /* RTW Generated Level2 S-Function Block: '<S143>/Generated S-Function1' (SaturationLimiter_sf) */
26777 {
26778 SimStruct *rts = ssGetSFunction(S, 76);
26779 if (rts != (NULL) ) {
26780 rt_FREE(ssGetSampleTimePtr(rts));
26781 rt_FREE(ssGetOffsetTimePtr(rts));
26782 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26783
26784 /* Destroy model methods 3 */
26785 {
26786 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26787 rt_FREE(methods3);
26788 }
26789
26790 /* Destroy model methods 2 */
26791 {
26792 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26793 rt_FREE(methods2);
26794 }
26795
26796 /* Destroy state auxilliary information */
26797 {
26798 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26799 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26800 rt_FREE(periodicStatesInfo);
26801 rt_FREE(statesInfo2);
26802 }
26803
26804 {
26805 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26806 rt_FREE(ptr);
26807 }
26808
26809 {
26810 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26811 rt_FREE(ptr);
26812 }
26813
26814 {
26815 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
26816 rt_FREE(ptr);
26817 }
26818
26819 rt_FREE(ssGetPortInfoForInputs(rts));
26820 rt_FREE(ssGetPortInfoForOutputs(rts));
26821 }
26822 }
26823
26824 /* RTW Generated Level2 S-Function Block: '<S127>/Generated S-Function1' (Res180_sf) */
26825 {
26826 SimStruct *rts = ssGetSFunction(S, 77);
26827 if (rts != (NULL) ) {
26828 rt_FREE(ssGetSampleTimePtr(rts));
26829 rt_FREE(ssGetOffsetTimePtr(rts));
26830 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26831
26832 /* Destroy model methods 3 */
26833 {
26834 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26835 rt_FREE(methods3);
26836 }
26837
26838 /* Destroy model methods 2 */
26839 {
26840 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26841 rt_FREE(methods2);
26842 }
26843
26844 /* Destroy state auxilliary information */
26845 {
26846 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26847 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26848 rt_FREE(periodicStatesInfo);
26849 rt_FREE(statesInfo2);
26850 }
26851
26852 {
26853 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26854 rt_FREE(ptr);
26855 }
26856
26857 rt_FREE(ssGetPortInfoForInputs(rts));
26858 rt_FREE(ssGetPortInfoForOutputs(rts));
26859 }
26860 }
26861
26862 /* RTW Generated Level2 S-Function Block: '<S146>/Generated S-Function' (Gain_sf) */
26863 {
26864 SimStruct *rts = ssGetSFunction(S, 78);
26865 if (rts != (NULL) ) {
26866 rt_FREE(ssGetSampleTimePtr(rts));
26867 rt_FREE(ssGetOffsetTimePtr(rts));
26868 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26869
26870 /* Destroy model methods 3 */
26871 {
26872 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26873 rt_FREE(methods3);
26874 }
26875
26876 /* Destroy model methods 2 */
26877 {
26878 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26879 rt_FREE(methods2);
26880 }
26881
26882 /* Destroy state auxilliary information */
26883 {
26884 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26885 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26886 rt_FREE(periodicStatesInfo);
26887 rt_FREE(statesInfo2);
26888 }
26889
26890 {
26891 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26892 rt_FREE(ptr);
26893 }
26894
26895 {
26896 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26897 rt_FREE(ptr);
26898 }
26899
26900 rt_FREE(ssGetPortInfoForInputs(rts));
26901 rt_FREE(ssGetPortInfoForOutputs(rts));
26902 }
26903 }
26904
26905 /* RTW Generated Level2 S-Function Block: '<S147>/Generated S-Function' (Gain_sf) */
26906 {
26907 SimStruct *rts = ssGetSFunction(S, 79);
26908 if (rts != (NULL) ) {
26909 rt_FREE(ssGetSampleTimePtr(rts));
26910 rt_FREE(ssGetOffsetTimePtr(rts));
26911 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26912
26913 /* Destroy model methods 3 */
26914 {
26915 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26916 rt_FREE(methods3);
26917 }
26918
26919 /* Destroy model methods 2 */
26920 {
26921 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26922 rt_FREE(methods2);
26923 }
26924
26925 /* Destroy state auxilliary information */
26926 {
26927 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26928 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26929 rt_FREE(periodicStatesInfo);
26930 rt_FREE(statesInfo2);
26931 }
26932
26933 {
26934 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26935 rt_FREE(ptr);
26936 }
26937
26938 {
26939 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26940 rt_FREE(ptr);
26941 }
26942
26943 rt_FREE(ssGetPortInfoForInputs(rts));
26944 rt_FREE(ssGetPortInfoForOutputs(rts));
26945 }
26946 }
26947
26948 /* RTW Generated Level2 S-Function Block: '<S140>/Generated S-Function3' (IntegratorLimited_sf) */
26949 {
26950 SimStruct *rts = ssGetSFunction(S, 80);
26951 if (rts != (NULL) ) {
26952 rt_FREE(ssGetSampleTimePtr(rts));
26953 rt_FREE(ssGetOffsetTimePtr(rts));
26954 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
26955
26956 /* Destroy model methods 3 */
26957 {
26958 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
26959 rt_FREE(methods3);
26960 }
26961
26962 /* Destroy model methods 2 */
26963 {
26964 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
26965 rt_FREE(methods2);
26966 }
26967
26968 /* Destroy state auxilliary information */
26969 {
26970 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
26971 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
26972 rt_FREE(periodicStatesInfo);
26973 rt_FREE(statesInfo2);
26974 }
26975
26976 {
26977 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
26978 rt_FREE(ptr);
26979 }
26980
26981 {
26982 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
26983 rt_FREE(ptr);
26984 }
26985
26986 {
26987 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
26988 rt_FREE(ptr);
26989 }
26990
26991 {
26992 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
26993 rt_FREE(ptr);
26994 }
26995
26996 {
26997 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
26998 rt_FREE(ptr);
26999 }
27000
27001 {
27002 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
27003 rt_FREE(ptr);
27004 }
27005
27006 {
27007 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
27008 rt_FREE(ptr);
27009 }
27010
27011 rt_FREE(ssGetPortInfoForInputs(rts));
27012 rt_FREE(ssGetPortInfoForOutputs(rts));
27013 rt_FREE(ssGetSFcnDWork(rts));
27014 }
27015 }
27016
27017 /* RTW Generated Level2 S-Function Block: '<S141>/Generated S-Function2' (LeadLagFilter_sf) */
27018 {
27019 SimStruct *rts = ssGetSFunction(S, 81);
27020 if (rts != (NULL) ) {
27021 rt_FREE(ssGetSampleTimePtr(rts));
27022 rt_FREE(ssGetOffsetTimePtr(rts));
27023 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27024
27025 /* Destroy model methods 3 */
27026 {
27027 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27028 rt_FREE(methods3);
27029 }
27030
27031 /* Destroy model methods 2 */
27032 {
27033 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27034 rt_FREE(methods2);
27035 }
27036
27037 /* Destroy state auxilliary information */
27038 {
27039 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27040 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27041 rt_FREE(periodicStatesInfo);
27042 rt_FREE(statesInfo2);
27043 }
27044
27045 {
27046 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27047 rt_FREE(ptr);
27048 }
27049
27050 {
27051 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27052 rt_FREE(ptr);
27053 }
27054
27055 {
27056 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
27057 rt_FREE(ptr);
27058 }
27059
27060 {
27061 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
27062 rt_FREE(ptr);
27063 }
27064
27065 {
27066 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
27067 rt_FREE(ptr);
27068 }
27069
27070 {
27071 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
27072 rt_FREE(ptr);
27073 }
27074
27075 {
27076 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
27077 rt_FREE(ptr);
27078 }
27079
27080 rt_FREE(ssGetPortInfoForInputs(rts));
27081 rt_FREE(ssGetPortInfoForOutputs(rts));
27082 rt_FREE(ssGetSFcnDWork(rts));
27083 }
27084 }
27085
27086 /* RTW Generated Level2 S-Function Block: '<S139>/Generated S-Function4' (LagFilter_sf) */
27087 {
27088 SimStruct *rts = ssGetSFunction(S, 82);
27089 if (rts != (NULL) ) {
27090 rt_FREE(ssGetSampleTimePtr(rts));
27091 rt_FREE(ssGetOffsetTimePtr(rts));
27092 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27093
27094 /* Destroy model methods 3 */
27095 {
27096 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27097 rt_FREE(methods3);
27098 }
27099
27100 /* Destroy model methods 2 */
27101 {
27102 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27103 rt_FREE(methods2);
27104 }
27105
27106 /* Destroy state auxilliary information */
27107 {
27108 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27109 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27110 rt_FREE(periodicStatesInfo);
27111 rt_FREE(statesInfo2);
27112 }
27113
27114 {
27115 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27116 rt_FREE(ptr);
27117 }
27118
27119 {
27120 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27121 rt_FREE(ptr);
27122 }
27123
27124 {
27125 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
27126 rt_FREE(ptr);
27127 }
27128
27129 {
27130 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
27131 rt_FREE(ptr);
27132 }
27133
27134 {
27135 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
27136 rt_FREE(ptr);
27137 }
27138
27139 {
27140 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
27141 rt_FREE(ptr);
27142 }
27143
27144 rt_FREE(ssGetPortInfoForInputs(rts));
27145 rt_FREE(ssGetPortInfoForOutputs(rts));
27146 rt_FREE(ssGetSFcnDWork(rts));
27147 }
27148 }
27149
27150 /* RTW Generated Level2 S-Function Block: '<S164>/Generated S-Function1' (SaturationLimiter_sf) */
27151 {
27152 SimStruct *rts = ssGetSFunction(S, 83);
27153 if (rts != (NULL) ) {
27154 rt_FREE(ssGetSampleTimePtr(rts));
27155 rt_FREE(ssGetOffsetTimePtr(rts));
27156 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27157
27158 /* Destroy model methods 3 */
27159 {
27160 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27161 rt_FREE(methods3);
27162 }
27163
27164 /* Destroy model methods 2 */
27165 {
27166 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27167 rt_FREE(methods2);
27168 }
27169
27170 /* Destroy state auxilliary information */
27171 {
27172 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27173 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27174 rt_FREE(periodicStatesInfo);
27175 rt_FREE(statesInfo2);
27176 }
27177
27178 {
27179 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27180 rt_FREE(ptr);
27181 }
27182
27183 {
27184 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27185 rt_FREE(ptr);
27186 }
27187
27188 {
27189 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
27190 rt_FREE(ptr);
27191 }
27192
27193 rt_FREE(ssGetPortInfoForInputs(rts));
27194 rt_FREE(ssGetPortInfoForOutputs(rts));
27195 }
27196 }
27197
27198 /* RTW Generated Level2 S-Function Block: '<S168>/Generated S-Function' (Gain_sf) */
27199 {
27200 SimStruct *rts = ssGetSFunction(S, 84);
27201 if (rts != (NULL) ) {
27202 rt_FREE(ssGetSampleTimePtr(rts));
27203 rt_FREE(ssGetOffsetTimePtr(rts));
27204 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27205
27206 /* Destroy model methods 3 */
27207 {
27208 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27209 rt_FREE(methods3);
27210 }
27211
27212 /* Destroy model methods 2 */
27213 {
27214 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27215 rt_FREE(methods2);
27216 }
27217
27218 /* Destroy state auxilliary information */
27219 {
27220 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27221 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27222 rt_FREE(periodicStatesInfo);
27223 rt_FREE(statesInfo2);
27224 }
27225
27226 {
27227 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27228 rt_FREE(ptr);
27229 }
27230
27231 {
27232 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27233 rt_FREE(ptr);
27234 }
27235
27236 rt_FREE(ssGetPortInfoForInputs(rts));
27237 rt_FREE(ssGetPortInfoForOutputs(rts));
27238 }
27239 }
27240
27241 /* RTW Generated Level2 S-Function Block: '<S169>/Generated S-Function' (Gain_sf) */
27242 {
27243 SimStruct *rts = ssGetSFunction(S, 85);
27244 if (rts != (NULL) ) {
27245 rt_FREE(ssGetSampleTimePtr(rts));
27246 rt_FREE(ssGetOffsetTimePtr(rts));
27247 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27248
27249 /* Destroy model methods 3 */
27250 {
27251 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27252 rt_FREE(methods3);
27253 }
27254
27255 /* Destroy model methods 2 */
27256 {
27257 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27258 rt_FREE(methods2);
27259 }
27260
27261 /* Destroy state auxilliary information */
27262 {
27263 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27264 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27265 rt_FREE(periodicStatesInfo);
27266 rt_FREE(statesInfo2);
27267 }
27268
27269 {
27270 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27271 rt_FREE(ptr);
27272 }
27273
27274 {
27275 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27276 rt_FREE(ptr);
27277 }
27278
27279 rt_FREE(ssGetPortInfoForInputs(rts));
27280 rt_FREE(ssGetPortInfoForOutputs(rts));
27281 }
27282 }
27283
27284 /* RTW Generated Level2 S-Function Block: '<S160>/Generated S-Function3' (IntegratorLimited_sf) */
27285 {
27286 SimStruct *rts = ssGetSFunction(S, 86);
27287 if (rts != (NULL) ) {
27288 rt_FREE(ssGetSampleTimePtr(rts));
27289 rt_FREE(ssGetOffsetTimePtr(rts));
27290 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27291
27292 /* Destroy model methods 3 */
27293 {
27294 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27295 rt_FREE(methods3);
27296 }
27297
27298 /* Destroy model methods 2 */
27299 {
27300 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27301 rt_FREE(methods2);
27302 }
27303
27304 /* Destroy state auxilliary information */
27305 {
27306 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27307 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27308 rt_FREE(periodicStatesInfo);
27309 rt_FREE(statesInfo2);
27310 }
27311
27312 {
27313 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27314 rt_FREE(ptr);
27315 }
27316
27317 {
27318 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27319 rt_FREE(ptr);
27320 }
27321
27322 {
27323 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
27324 rt_FREE(ptr);
27325 }
27326
27327 {
27328 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
27329 rt_FREE(ptr);
27330 }
27331
27332 {
27333 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
27334 rt_FREE(ptr);
27335 }
27336
27337 {
27338 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
27339 rt_FREE(ptr);
27340 }
27341
27342 {
27343 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
27344 rt_FREE(ptr);
27345 }
27346
27347 rt_FREE(ssGetPortInfoForInputs(rts));
27348 rt_FREE(ssGetPortInfoForOutputs(rts));
27349 rt_FREE(ssGetSFcnDWork(rts));
27350 }
27351 }
27352
27353 /* RTW Generated Level2 S-Function Block: '<S129>/Generated S-Function6' (Res180_sf) */
27354 {
27355 SimStruct *rts = ssGetSFunction(S, 87);
27356 if (rts != (NULL) ) {
27357 rt_FREE(ssGetSampleTimePtr(rts));
27358 rt_FREE(ssGetOffsetTimePtr(rts));
27359 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27360
27361 /* Destroy model methods 3 */
27362 {
27363 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27364 rt_FREE(methods3);
27365 }
27366
27367 /* Destroy model methods 2 */
27368 {
27369 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27370 rt_FREE(methods2);
27371 }
27372
27373 /* Destroy state auxilliary information */
27374 {
27375 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27376 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27377 rt_FREE(periodicStatesInfo);
27378 rt_FREE(statesInfo2);
27379 }
27380
27381 {
27382 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27383 rt_FREE(ptr);
27384 }
27385
27386 rt_FREE(ssGetPortInfoForInputs(rts));
27387 rt_FREE(ssGetPortInfoForOutputs(rts));
27388 }
27389 }
27390
27391 /* RTW Generated Level2 S-Function Block: '<S162>/Generated S-Function1' (SaturationLimiter_sf) */
27392 {
27393 SimStruct *rts = ssGetSFunction(S, 88);
27394 if (rts != (NULL) ) {
27395 rt_FREE(ssGetSampleTimePtr(rts));
27396 rt_FREE(ssGetOffsetTimePtr(rts));
27397 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27398
27399 /* Destroy model methods 3 */
27400 {
27401 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27402 rt_FREE(methods3);
27403 }
27404
27405 /* Destroy model methods 2 */
27406 {
27407 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27408 rt_FREE(methods2);
27409 }
27410
27411 /* Destroy state auxilliary information */
27412 {
27413 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27414 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27415 rt_FREE(periodicStatesInfo);
27416 rt_FREE(statesInfo2);
27417 }
27418
27419 {
27420 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27421 rt_FREE(ptr);
27422 }
27423
27424 {
27425 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27426 rt_FREE(ptr);
27427 }
27428
27429 {
27430 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
27431 rt_FREE(ptr);
27432 }
27433
27434 rt_FREE(ssGetPortInfoForInputs(rts));
27435 rt_FREE(ssGetPortInfoForOutputs(rts));
27436 }
27437 }
27438
27439 /* RTW Generated Level2 S-Function Block: '<S167>/Generated S-Function' (Gain_sf) */
27440 {
27441 SimStruct *rts = ssGetSFunction(S, 89);
27442 if (rts != (NULL) ) {
27443 rt_FREE(ssGetSampleTimePtr(rts));
27444 rt_FREE(ssGetOffsetTimePtr(rts));
27445 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27446
27447 /* Destroy model methods 3 */
27448 {
27449 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27450 rt_FREE(methods3);
27451 }
27452
27453 /* Destroy model methods 2 */
27454 {
27455 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27456 rt_FREE(methods2);
27457 }
27458
27459 /* Destroy state auxilliary information */
27460 {
27461 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27462 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27463 rt_FREE(periodicStatesInfo);
27464 rt_FREE(statesInfo2);
27465 }
27466
27467 {
27468 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27469 rt_FREE(ptr);
27470 }
27471
27472 {
27473 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27474 rt_FREE(ptr);
27475 }
27476
27477 rt_FREE(ssGetPortInfoForInputs(rts));
27478 rt_FREE(ssGetPortInfoForOutputs(rts));
27479 }
27480 }
27481
27482 /* RTW Generated Level2 S-Function Block: '<S161>/Generated S-Function1' (SaturationLimiter_sf) */
27483 {
27484 SimStruct *rts = ssGetSFunction(S, 90);
27485 if (rts != (NULL) ) {
27486 rt_FREE(ssGetSampleTimePtr(rts));
27487 rt_FREE(ssGetOffsetTimePtr(rts));
27488 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27489
27490 /* Destroy model methods 3 */
27491 {
27492 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27493 rt_FREE(methods3);
27494 }
27495
27496 /* Destroy model methods 2 */
27497 {
27498 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27499 rt_FREE(methods2);
27500 }
27501
27502 /* Destroy state auxilliary information */
27503 {
27504 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27505 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27506 rt_FREE(periodicStatesInfo);
27507 rt_FREE(statesInfo2);
27508 }
27509
27510 {
27511 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27512 rt_FREE(ptr);
27513 }
27514
27515 {
27516 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27517 rt_FREE(ptr);
27518 }
27519
27520 {
27521 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
27522 rt_FREE(ptr);
27523 }
27524
27525 rt_FREE(ssGetPortInfoForInputs(rts));
27526 rt_FREE(ssGetPortInfoForOutputs(rts));
27527 }
27528 }
27529
27530 /* RTW Generated Level2 S-Function Block: '<S129>/Generated S-Function1' (Res180_sf) */
27531 {
27532 SimStruct *rts = ssGetSFunction(S, 91);
27533 if (rts != (NULL) ) {
27534 rt_FREE(ssGetSampleTimePtr(rts));
27535 rt_FREE(ssGetOffsetTimePtr(rts));
27536 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27537
27538 /* Destroy model methods 3 */
27539 {
27540 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27541 rt_FREE(methods3);
27542 }
27543
27544 /* Destroy model methods 2 */
27545 {
27546 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27547 rt_FREE(methods2);
27548 }
27549
27550 /* Destroy state auxilliary information */
27551 {
27552 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27553 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27554 rt_FREE(periodicStatesInfo);
27555 rt_FREE(statesInfo2);
27556 }
27557
27558 {
27559 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27560 rt_FREE(ptr);
27561 }
27562
27563 rt_FREE(ssGetPortInfoForInputs(rts));
27564 rt_FREE(ssGetPortInfoForOutputs(rts));
27565 }
27566 }
27567
27568 /* RTW Generated Level2 S-Function Block: '<S166>/Generated S-Function' (Gain_sf) */
27569 {
27570 SimStruct *rts = ssGetSFunction(S, 92);
27571 if (rts != (NULL) ) {
27572 rt_FREE(ssGetSampleTimePtr(rts));
27573 rt_FREE(ssGetOffsetTimePtr(rts));
27574 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27575
27576 /* Destroy model methods 3 */
27577 {
27578 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27579 rt_FREE(methods3);
27580 }
27581
27582 /* Destroy model methods 2 */
27583 {
27584 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27585 rt_FREE(methods2);
27586 }
27587
27588 /* Destroy state auxilliary information */
27589 {
27590 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27591 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27592 rt_FREE(periodicStatesInfo);
27593 rt_FREE(statesInfo2);
27594 }
27595
27596 {
27597 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27598 rt_FREE(ptr);
27599 }
27600
27601 {
27602 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27603 rt_FREE(ptr);
27604 }
27605
27606 rt_FREE(ssGetPortInfoForInputs(rts));
27607 rt_FREE(ssGetPortInfoForOutputs(rts));
27608 }
27609 }
27610
27611 /* RTW Generated Level2 S-Function Block: '<S159>/Generated S-Function4' (LagFilter_sf) */
27612 {
27613 SimStruct *rts = ssGetSFunction(S, 93);
27614 if (rts != (NULL) ) {
27615 rt_FREE(ssGetSampleTimePtr(rts));
27616 rt_FREE(ssGetOffsetTimePtr(rts));
27617 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27618
27619 /* Destroy model methods 3 */
27620 {
27621 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27622 rt_FREE(methods3);
27623 }
27624
27625 /* Destroy model methods 2 */
27626 {
27627 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27628 rt_FREE(methods2);
27629 }
27630
27631 /* Destroy state auxilliary information */
27632 {
27633 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27634 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27635 rt_FREE(periodicStatesInfo);
27636 rt_FREE(statesInfo2);
27637 }
27638
27639 {
27640 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27641 rt_FREE(ptr);
27642 }
27643
27644 {
27645 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27646 rt_FREE(ptr);
27647 }
27648
27649 {
27650 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
27651 rt_FREE(ptr);
27652 }
27653
27654 {
27655 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
27656 rt_FREE(ptr);
27657 }
27658
27659 {
27660 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
27661 rt_FREE(ptr);
27662 }
27663
27664 {
27665 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
27666 rt_FREE(ptr);
27667 }
27668
27669 rt_FREE(ssGetPortInfoForInputs(rts));
27670 rt_FREE(ssGetPortInfoForOutputs(rts));
27671 rt_FREE(ssGetSFcnDWork(rts));
27672 }
27673 }
27674
27675 /* RTW Generated Level2 S-Function Block: '<S123>/Generated S-Function5' (Fader_sf) */
27676 {
27677 SimStruct *rts = ssGetSFunction(S, 94);
27678 if (rts != (NULL) ) {
27679 rt_FREE(ssGetSampleTimePtr(rts));
27680 rt_FREE(ssGetOffsetTimePtr(rts));
27681 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27682
27683 /* Destroy model methods 3 */
27684 {
27685 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27686 rt_FREE(methods3);
27687 }
27688
27689 /* Destroy model methods 2 */
27690 {
27691 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27692 rt_FREE(methods2);
27693 }
27694
27695 /* Destroy state auxilliary information */
27696 {
27697 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27698 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27699 rt_FREE(periodicStatesInfo);
27700 rt_FREE(statesInfo2);
27701 }
27702
27703 {
27704 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27705 rt_FREE(ptr);
27706 }
27707
27708 {
27709 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27710 rt_FREE(ptr);
27711 }
27712
27713 {
27714 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
27715 rt_FREE(ptr);
27716 }
27717
27718 {
27719 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
27720 rt_FREE(ptr);
27721 }
27722
27723 {
27724 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
27725 rt_FREE(ptr);
27726 }
27727
27728 {
27729 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
27730 rt_FREE(ptr);
27731 }
27732
27733 rt_FREE(ssGetPortInfoForInputs(rts));
27734 rt_FREE(ssGetPortInfoForOutputs(rts));
27735 rt_FREE(ssGetSFcnDWork(rts));
27736 }
27737 }
27738
27739 /* RTW Generated Level2 S-Function Block: '<S163>/Generated S-Function1' (SaturationLimiter_sf) */
27740 {
27741 SimStruct *rts = ssGetSFunction(S, 95);
27742 if (rts != (NULL) ) {
27743 rt_FREE(ssGetSampleTimePtr(rts));
27744 rt_FREE(ssGetOffsetTimePtr(rts));
27745 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27746
27747 /* Destroy model methods 3 */
27748 {
27749 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27750 rt_FREE(methods3);
27751 }
27752
27753 /* Destroy model methods 2 */
27754 {
27755 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27756 rt_FREE(methods2);
27757 }
27758
27759 /* Destroy state auxilliary information */
27760 {
27761 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27762 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27763 rt_FREE(periodicStatesInfo);
27764 rt_FREE(statesInfo2);
27765 }
27766
27767 {
27768 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27769 rt_FREE(ptr);
27770 }
27771
27772 {
27773 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27774 rt_FREE(ptr);
27775 }
27776
27777 {
27778 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
27779 rt_FREE(ptr);
27780 }
27781
27782 rt_FREE(ssGetPortInfoForInputs(rts));
27783 rt_FREE(ssGetPortInfoForOutputs(rts));
27784 }
27785 }
27786
27787 /* RTW Generated Level2 S-Function Block: '<S165>/Generated S-Function1' (SaturationLimiter_sf) */
27788 {
27789 SimStruct *rts = ssGetSFunction(S, 96);
27790 if (rts != (NULL) ) {
27791 rt_FREE(ssGetSampleTimePtr(rts));
27792 rt_FREE(ssGetOffsetTimePtr(rts));
27793 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27794
27795 /* Destroy model methods 3 */
27796 {
27797 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27798 rt_FREE(methods3);
27799 }
27800
27801 /* Destroy model methods 2 */
27802 {
27803 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27804 rt_FREE(methods2);
27805 }
27806
27807 /* Destroy state auxilliary information */
27808 {
27809 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27810 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27811 rt_FREE(periodicStatesInfo);
27812 rt_FREE(statesInfo2);
27813 }
27814
27815 {
27816 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27817 rt_FREE(ptr);
27818 }
27819
27820 {
27821 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27822 rt_FREE(ptr);
27823 }
27824
27825 {
27826 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
27827 rt_FREE(ptr);
27828 }
27829
27830 rt_FREE(ssGetPortInfoForInputs(rts));
27831 rt_FREE(ssGetPortInfoForOutputs(rts));
27832 }
27833 }
27834
27835 /* RTW Generated Level2 S-Function Block: '<S190>/Generated S-Function4' (LagFilter_sf) */
27836 {
27837 SimStruct *rts = ssGetSFunction(S, 97);
27838 if (rts != (NULL) ) {
27839 rt_FREE(ssGetSampleTimePtr(rts));
27840 rt_FREE(ssGetOffsetTimePtr(rts));
27841 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27842
27843 /* Destroy model methods 3 */
27844 {
27845 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27846 rt_FREE(methods3);
27847 }
27848
27849 /* Destroy model methods 2 */
27850 {
27851 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27852 rt_FREE(methods2);
27853 }
27854
27855 /* Destroy state auxilliary information */
27856 {
27857 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27858 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27859 rt_FREE(periodicStatesInfo);
27860 rt_FREE(statesInfo2);
27861 }
27862
27863 {
27864 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27865 rt_FREE(ptr);
27866 }
27867
27868 {
27869 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27870 rt_FREE(ptr);
27871 }
27872
27873 {
27874 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
27875 rt_FREE(ptr);
27876 }
27877
27878 {
27879 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
27880 rt_FREE(ptr);
27881 }
27882
27883 {
27884 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
27885 rt_FREE(ptr);
27886 }
27887
27888 {
27889 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
27890 rt_FREE(ptr);
27891 }
27892
27893 rt_FREE(ssGetPortInfoForInputs(rts));
27894 rt_FREE(ssGetPortInfoForOutputs(rts));
27895 rt_FREE(ssGetSFcnDWork(rts));
27896 }
27897 }
27898
27899 /* RTW Generated Level2 S-Function Block: '<S191>/Generated S-Function4' (LagFilter_sf) */
27900 {
27901 SimStruct *rts = ssGetSFunction(S, 98);
27902 if (rts != (NULL) ) {
27903 rt_FREE(ssGetSampleTimePtr(rts));
27904 rt_FREE(ssGetOffsetTimePtr(rts));
27905 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27906
27907 /* Destroy model methods 3 */
27908 {
27909 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27910 rt_FREE(methods3);
27911 }
27912
27913 /* Destroy model methods 2 */
27914 {
27915 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27916 rt_FREE(methods2);
27917 }
27918
27919 /* Destroy state auxilliary information */
27920 {
27921 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27922 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27923 rt_FREE(periodicStatesInfo);
27924 rt_FREE(statesInfo2);
27925 }
27926
27927 {
27928 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27929 rt_FREE(ptr);
27930 }
27931
27932 {
27933 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
27934 rt_FREE(ptr);
27935 }
27936
27937 {
27938 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
27939 rt_FREE(ptr);
27940 }
27941
27942 {
27943 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
27944 rt_FREE(ptr);
27945 }
27946
27947 {
27948 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
27949 rt_FREE(ptr);
27950 }
27951
27952 {
27953 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
27954 rt_FREE(ptr);
27955 }
27956
27957 rt_FREE(ssGetPortInfoForInputs(rts));
27958 rt_FREE(ssGetPortInfoForOutputs(rts));
27959 rt_FREE(ssGetSFcnDWork(rts));
27960 }
27961 }
27962
27963 /* RTW Generated Level2 S-Function Block: '<S193>/Generated S-Function' (Res180_sf) */
27964 {
27965 SimStruct *rts = ssGetSFunction(S, 99);
27966 if (rts != (NULL) ) {
27967 rt_FREE(ssGetSampleTimePtr(rts));
27968 rt_FREE(ssGetOffsetTimePtr(rts));
27969 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
27970
27971 /* Destroy model methods 3 */
27972 {
27973 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
27974 rt_FREE(methods3);
27975 }
27976
27977 /* Destroy model methods 2 */
27978 {
27979 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
27980 rt_FREE(methods2);
27981 }
27982
27983 /* Destroy state auxilliary information */
27984 {
27985 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
27986 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
27987 rt_FREE(periodicStatesInfo);
27988 rt_FREE(statesInfo2);
27989 }
27990
27991 {
27992 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
27993 rt_FREE(ptr);
27994 }
27995
27996 rt_FREE(ssGetPortInfoForInputs(rts));
27997 rt_FREE(ssGetPortInfoForOutputs(rts));
27998 }
27999 }
28000
28001 /* RTW Generated Level2 S-Function Block: '<S198>/Generated S-Function1' (SaturationLimiter_sf) */
28002 {
28003 SimStruct *rts = ssGetSFunction(S, 100);
28004 if (rts != (NULL) ) {
28005 rt_FREE(ssGetSampleTimePtr(rts));
28006 rt_FREE(ssGetOffsetTimePtr(rts));
28007 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28008
28009 /* Destroy model methods 3 */
28010 {
28011 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28012 rt_FREE(methods3);
28013 }
28014
28015 /* Destroy model methods 2 */
28016 {
28017 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28018 rt_FREE(methods2);
28019 }
28020
28021 /* Destroy state auxilliary information */
28022 {
28023 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28024 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28025 rt_FREE(periodicStatesInfo);
28026 rt_FREE(statesInfo2);
28027 }
28028
28029 {
28030 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28031 rt_FREE(ptr);
28032 }
28033
28034 {
28035 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28036 rt_FREE(ptr);
28037 }
28038
28039 {
28040 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
28041 rt_FREE(ptr);
28042 }
28043
28044 rt_FREE(ssGetPortInfoForInputs(rts));
28045 rt_FREE(ssGetPortInfoForOutputs(rts));
28046 }
28047 }
28048
28049 /* RTW Generated Level2 S-Function Block: '<S199>/Generated S-Function' (Gain_sf) */
28050 {
28051 SimStruct *rts = ssGetSFunction(S, 101);
28052 if (rts != (NULL) ) {
28053 rt_FREE(ssGetSampleTimePtr(rts));
28054 rt_FREE(ssGetOffsetTimePtr(rts));
28055 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28056
28057 /* Destroy model methods 3 */
28058 {
28059 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28060 rt_FREE(methods3);
28061 }
28062
28063 /* Destroy model methods 2 */
28064 {
28065 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28066 rt_FREE(methods2);
28067 }
28068
28069 /* Destroy state auxilliary information */
28070 {
28071 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28072 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28073 rt_FREE(periodicStatesInfo);
28074 rt_FREE(statesInfo2);
28075 }
28076
28077 {
28078 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28079 rt_FREE(ptr);
28080 }
28081
28082 {
28083 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28084 rt_FREE(ptr);
28085 }
28086
28087 rt_FREE(ssGetPortInfoForInputs(rts));
28088 rt_FREE(ssGetPortInfoForOutputs(rts));
28089 }
28090 }
28091
28092 /* RTW Generated Level2 S-Function Block: '<S197>/Generated S-Function2' (LeadLagFilter_sf) */
28093 {
28094 SimStruct *rts = ssGetSFunction(S, 102);
28095 if (rts != (NULL) ) {
28096 rt_FREE(ssGetSampleTimePtr(rts));
28097 rt_FREE(ssGetOffsetTimePtr(rts));
28098 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28099
28100 /* Destroy model methods 3 */
28101 {
28102 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28103 rt_FREE(methods3);
28104 }
28105
28106 /* Destroy model methods 2 */
28107 {
28108 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28109 rt_FREE(methods2);
28110 }
28111
28112 /* Destroy state auxilliary information */
28113 {
28114 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28115 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28116 rt_FREE(periodicStatesInfo);
28117 rt_FREE(statesInfo2);
28118 }
28119
28120 {
28121 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28122 rt_FREE(ptr);
28123 }
28124
28125 {
28126 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28127 rt_FREE(ptr);
28128 }
28129
28130 {
28131 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
28132 rt_FREE(ptr);
28133 }
28134
28135 {
28136 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
28137 rt_FREE(ptr);
28138 }
28139
28140 {
28141 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
28142 rt_FREE(ptr);
28143 }
28144
28145 {
28146 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
28147 rt_FREE(ptr);
28148 }
28149
28150 {
28151 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
28152 rt_FREE(ptr);
28153 }
28154
28155 rt_FREE(ssGetPortInfoForInputs(rts));
28156 rt_FREE(ssGetPortInfoForOutputs(rts));
28157 rt_FREE(ssGetSFcnDWork(rts));
28158 }
28159 }
28160
28161 /* RTW Generated Level2 S-Function Block: '<S196>/Generated S-Function4' (LagFilter_sf) */
28162 {
28163 SimStruct *rts = ssGetSFunction(S, 103);
28164 if (rts != (NULL) ) {
28165 rt_FREE(ssGetSampleTimePtr(rts));
28166 rt_FREE(ssGetOffsetTimePtr(rts));
28167 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28168
28169 /* Destroy model methods 3 */
28170 {
28171 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28172 rt_FREE(methods3);
28173 }
28174
28175 /* Destroy model methods 2 */
28176 {
28177 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28178 rt_FREE(methods2);
28179 }
28180
28181 /* Destroy state auxilliary information */
28182 {
28183 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28184 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28185 rt_FREE(periodicStatesInfo);
28186 rt_FREE(statesInfo2);
28187 }
28188
28189 {
28190 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28191 rt_FREE(ptr);
28192 }
28193
28194 {
28195 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28196 rt_FREE(ptr);
28197 }
28198
28199 {
28200 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
28201 rt_FREE(ptr);
28202 }
28203
28204 {
28205 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
28206 rt_FREE(ptr);
28207 }
28208
28209 {
28210 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
28211 rt_FREE(ptr);
28212 }
28213
28214 {
28215 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
28216 rt_FREE(ptr);
28217 }
28218
28219 rt_FREE(ssGetPortInfoForInputs(rts));
28220 rt_FREE(ssGetPortInfoForOutputs(rts));
28221 rt_FREE(ssGetSFcnDWork(rts));
28222 }
28223 }
28224
28225 /* RTW Generated Level2 S-Function Block: '<S204>/Generated S-Function' (Gain_sf) */
28226 {
28227 SimStruct *rts = ssGetSFunction(S, 104);
28228 if (rts != (NULL) ) {
28229 rt_FREE(ssGetSampleTimePtr(rts));
28230 rt_FREE(ssGetOffsetTimePtr(rts));
28231 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28232
28233 /* Destroy model methods 3 */
28234 {
28235 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28236 rt_FREE(methods3);
28237 }
28238
28239 /* Destroy model methods 2 */
28240 {
28241 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28242 rt_FREE(methods2);
28243 }
28244
28245 /* Destroy state auxilliary information */
28246 {
28247 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28248 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28249 rt_FREE(periodicStatesInfo);
28250 rt_FREE(statesInfo2);
28251 }
28252
28253 {
28254 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28255 rt_FREE(ptr);
28256 }
28257
28258 {
28259 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28260 rt_FREE(ptr);
28261 }
28262
28263 rt_FREE(ssGetPortInfoForInputs(rts));
28264 rt_FREE(ssGetPortInfoForOutputs(rts));
28265 }
28266 }
28267
28268 /* RTW Generated Level2 S-Function Block: '<S203>/Generated S-Function1' (SaturationLimiter_sf) */
28269 {
28270 SimStruct *rts = ssGetSFunction(S, 105);
28271 if (rts != (NULL) ) {
28272 rt_FREE(ssGetSampleTimePtr(rts));
28273 rt_FREE(ssGetOffsetTimePtr(rts));
28274 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28275
28276 /* Destroy model methods 3 */
28277 {
28278 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28279 rt_FREE(methods3);
28280 }
28281
28282 /* Destroy model methods 2 */
28283 {
28284 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28285 rt_FREE(methods2);
28286 }
28287
28288 /* Destroy state auxilliary information */
28289 {
28290 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28291 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28292 rt_FREE(periodicStatesInfo);
28293 rt_FREE(statesInfo2);
28294 }
28295
28296 {
28297 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28298 rt_FREE(ptr);
28299 }
28300
28301 {
28302 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28303 rt_FREE(ptr);
28304 }
28305
28306 {
28307 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
28308 rt_FREE(ptr);
28309 }
28310
28311 rt_FREE(ssGetPortInfoForInputs(rts));
28312 rt_FREE(ssGetPortInfoForOutputs(rts));
28313 }
28314 }
28315
28316 /* RTW Generated Level2 S-Function Block: '<S206>/Generated S-Function' (Gain_sf) */
28317 {
28318 SimStruct *rts = ssGetSFunction(S, 106);
28319 if (rts != (NULL) ) {
28320 rt_FREE(ssGetSampleTimePtr(rts));
28321 rt_FREE(ssGetOffsetTimePtr(rts));
28322 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28323
28324 /* Destroy model methods 3 */
28325 {
28326 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28327 rt_FREE(methods3);
28328 }
28329
28330 /* Destroy model methods 2 */
28331 {
28332 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28333 rt_FREE(methods2);
28334 }
28335
28336 /* Destroy state auxilliary information */
28337 {
28338 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28339 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28340 rt_FREE(periodicStatesInfo);
28341 rt_FREE(statesInfo2);
28342 }
28343
28344 {
28345 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28346 rt_FREE(ptr);
28347 }
28348
28349 {
28350 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28351 rt_FREE(ptr);
28352 }
28353
28354 rt_FREE(ssGetPortInfoForInputs(rts));
28355 rt_FREE(ssGetPortInfoForOutputs(rts));
28356 }
28357 }
28358
28359 /* RTW Generated Level2 S-Function Block: '<S194>/Generated S-Function6' (Res180_sf) */
28360 {
28361 SimStruct *rts = ssGetSFunction(S, 107);
28362 if (rts != (NULL) ) {
28363 rt_FREE(ssGetSampleTimePtr(rts));
28364 rt_FREE(ssGetOffsetTimePtr(rts));
28365 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28366
28367 /* Destroy model methods 3 */
28368 {
28369 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28370 rt_FREE(methods3);
28371 }
28372
28373 /* Destroy model methods 2 */
28374 {
28375 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28376 rt_FREE(methods2);
28377 }
28378
28379 /* Destroy state auxilliary information */
28380 {
28381 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28382 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28383 rt_FREE(periodicStatesInfo);
28384 rt_FREE(statesInfo2);
28385 }
28386
28387 {
28388 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28389 rt_FREE(ptr);
28390 }
28391
28392 rt_FREE(ssGetPortInfoForInputs(rts));
28393 rt_FREE(ssGetPortInfoForOutputs(rts));
28394 }
28395 }
28396
28397 /* RTW Generated Level2 S-Function Block: '<S202>/Generated S-Function1' (SaturationLimiter_sf) */
28398 {
28399 SimStruct *rts = ssGetSFunction(S, 108);
28400 if (rts != (NULL) ) {
28401 rt_FREE(ssGetSampleTimePtr(rts));
28402 rt_FREE(ssGetOffsetTimePtr(rts));
28403 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28404
28405 /* Destroy model methods 3 */
28406 {
28407 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28408 rt_FREE(methods3);
28409 }
28410
28411 /* Destroy model methods 2 */
28412 {
28413 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28414 rt_FREE(methods2);
28415 }
28416
28417 /* Destroy state auxilliary information */
28418 {
28419 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28420 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28421 rt_FREE(periodicStatesInfo);
28422 rt_FREE(statesInfo2);
28423 }
28424
28425 {
28426 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28427 rt_FREE(ptr);
28428 }
28429
28430 {
28431 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28432 rt_FREE(ptr);
28433 }
28434
28435 {
28436 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
28437 rt_FREE(ptr);
28438 }
28439
28440 rt_FREE(ssGetPortInfoForInputs(rts));
28441 rt_FREE(ssGetPortInfoForOutputs(rts));
28442 }
28443 }
28444
28445 /* RTW Generated Level2 S-Function Block: '<S205>/Generated S-Function' (Gain_sf) */
28446 {
28447 SimStruct *rts = ssGetSFunction(S, 109);
28448 if (rts != (NULL) ) {
28449 rt_FREE(ssGetSampleTimePtr(rts));
28450 rt_FREE(ssGetOffsetTimePtr(rts));
28451 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28452
28453 /* Destroy model methods 3 */
28454 {
28455 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28456 rt_FREE(methods3);
28457 }
28458
28459 /* Destroy model methods 2 */
28460 {
28461 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28462 rt_FREE(methods2);
28463 }
28464
28465 /* Destroy state auxilliary information */
28466 {
28467 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28468 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28469 rt_FREE(periodicStatesInfo);
28470 rt_FREE(statesInfo2);
28471 }
28472
28473 {
28474 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28475 rt_FREE(ptr);
28476 }
28477
28478 {
28479 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28480 rt_FREE(ptr);
28481 }
28482
28483 rt_FREE(ssGetPortInfoForInputs(rts));
28484 rt_FREE(ssGetPortInfoForOutputs(rts));
28485 }
28486 }
28487
28488 /* RTW Generated Level2 S-Function Block: '<S201>/Generated S-Function2' (LeadLagFilter_sf) */
28489 {
28490 SimStruct *rts = ssGetSFunction(S, 110);
28491 if (rts != (NULL) ) {
28492 rt_FREE(ssGetSampleTimePtr(rts));
28493 rt_FREE(ssGetOffsetTimePtr(rts));
28494 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28495
28496 /* Destroy model methods 3 */
28497 {
28498 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28499 rt_FREE(methods3);
28500 }
28501
28502 /* Destroy model methods 2 */
28503 {
28504 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28505 rt_FREE(methods2);
28506 }
28507
28508 /* Destroy state auxilliary information */
28509 {
28510 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28511 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28512 rt_FREE(periodicStatesInfo);
28513 rt_FREE(statesInfo2);
28514 }
28515
28516 {
28517 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28518 rt_FREE(ptr);
28519 }
28520
28521 {
28522 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28523 rt_FREE(ptr);
28524 }
28525
28526 {
28527 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
28528 rt_FREE(ptr);
28529 }
28530
28531 {
28532 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
28533 rt_FREE(ptr);
28534 }
28535
28536 {
28537 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
28538 rt_FREE(ptr);
28539 }
28540
28541 {
28542 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
28543 rt_FREE(ptr);
28544 }
28545
28546 {
28547 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
28548 rt_FREE(ptr);
28549 }
28550
28551 rt_FREE(ssGetPortInfoForInputs(rts));
28552 rt_FREE(ssGetPortInfoForOutputs(rts));
28553 rt_FREE(ssGetSFcnDWork(rts));
28554 }
28555 }
28556
28557 /* RTW Generated Level2 S-Function Block: '<S200>/Generated S-Function4' (LagFilter_sf) */
28558 {
28559 SimStruct *rts = ssGetSFunction(S, 111);
28560 if (rts != (NULL) ) {
28561 rt_FREE(ssGetSampleTimePtr(rts));
28562 rt_FREE(ssGetOffsetTimePtr(rts));
28563 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28564
28565 /* Destroy model methods 3 */
28566 {
28567 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28568 rt_FREE(methods3);
28569 }
28570
28571 /* Destroy model methods 2 */
28572 {
28573 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28574 rt_FREE(methods2);
28575 }
28576
28577 /* Destroy state auxilliary information */
28578 {
28579 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28580 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28581 rt_FREE(periodicStatesInfo);
28582 rt_FREE(statesInfo2);
28583 }
28584
28585 {
28586 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28587 rt_FREE(ptr);
28588 }
28589
28590 {
28591 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28592 rt_FREE(ptr);
28593 }
28594
28595 {
28596 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
28597 rt_FREE(ptr);
28598 }
28599
28600 {
28601 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
28602 rt_FREE(ptr);
28603 }
28604
28605 {
28606 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
28607 rt_FREE(ptr);
28608 }
28609
28610 {
28611 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
28612 rt_FREE(ptr);
28613 }
28614
28615 rt_FREE(ssGetPortInfoForInputs(rts));
28616 rt_FREE(ssGetPortInfoForOutputs(rts));
28617 rt_FREE(ssGetSFcnDWork(rts));
28618 }
28619 }
28620
28621 /* RTW Generated Level2 S-Function Block: '<S69>/Generated S-Function4' (LagFilter_sf) */
28622 {
28623 SimStruct *rts = ssGetSFunction(S, 112);
28624 if (rts != (NULL) ) {
28625 rt_FREE(ssGetSampleTimePtr(rts));
28626 rt_FREE(ssGetOffsetTimePtr(rts));
28627 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28628
28629 /* Destroy model methods 3 */
28630 {
28631 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28632 rt_FREE(methods3);
28633 }
28634
28635 /* Destroy model methods 2 */
28636 {
28637 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28638 rt_FREE(methods2);
28639 }
28640
28641 /* Destroy state auxilliary information */
28642 {
28643 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28644 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28645 rt_FREE(periodicStatesInfo);
28646 rt_FREE(statesInfo2);
28647 }
28648
28649 {
28650 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28651 rt_FREE(ptr);
28652 }
28653
28654 {
28655 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28656 rt_FREE(ptr);
28657 }
28658
28659 {
28660 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
28661 rt_FREE(ptr);
28662 }
28663
28664 {
28665 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
28666 rt_FREE(ptr);
28667 }
28668
28669 {
28670 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
28671 rt_FREE(ptr);
28672 }
28673
28674 {
28675 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
28676 rt_FREE(ptr);
28677 }
28678
28679 rt_FREE(ssGetPortInfoForInputs(rts));
28680 rt_FREE(ssGetPortInfoForOutputs(rts));
28681 rt_FREE(ssGetSFcnDWork(rts));
28682 }
28683 }
28684
28685 /* RTW Generated Level2 S-Function Block: '<S70>/Generated S-Function4' (LagFilter_sf) */
28686 {
28687 SimStruct *rts = ssGetSFunction(S, 113);
28688 if (rts != (NULL) ) {
28689 rt_FREE(ssGetSampleTimePtr(rts));
28690 rt_FREE(ssGetOffsetTimePtr(rts));
28691 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28692
28693 /* Destroy model methods 3 */
28694 {
28695 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28696 rt_FREE(methods3);
28697 }
28698
28699 /* Destroy model methods 2 */
28700 {
28701 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28702 rt_FREE(methods2);
28703 }
28704
28705 /* Destroy state auxilliary information */
28706 {
28707 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28708 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28709 rt_FREE(periodicStatesInfo);
28710 rt_FREE(statesInfo2);
28711 }
28712
28713 {
28714 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28715 rt_FREE(ptr);
28716 }
28717
28718 {
28719 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28720 rt_FREE(ptr);
28721 }
28722
28723 {
28724 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
28725 rt_FREE(ptr);
28726 }
28727
28728 {
28729 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
28730 rt_FREE(ptr);
28731 }
28732
28733 {
28734 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
28735 rt_FREE(ptr);
28736 }
28737
28738 {
28739 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
28740 rt_FREE(ptr);
28741 }
28742
28743 rt_FREE(ssGetPortInfoForInputs(rts));
28744 rt_FREE(ssGetPortInfoForOutputs(rts));
28745 rt_FREE(ssGetSFcnDWork(rts));
28746 }
28747 }
28748
28749 /* RTW Generated Level2 S-Function Block: '<S77>/Generated S-Function1' (SaturationLimiter_sf) */
28750 {
28751 SimStruct *rts = ssGetSFunction(S, 114);
28752 if (rts != (NULL) ) {
28753 rt_FREE(ssGetSampleTimePtr(rts));
28754 rt_FREE(ssGetOffsetTimePtr(rts));
28755 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28756
28757 /* Destroy model methods 3 */
28758 {
28759 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28760 rt_FREE(methods3);
28761 }
28762
28763 /* Destroy model methods 2 */
28764 {
28765 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28766 rt_FREE(methods2);
28767 }
28768
28769 /* Destroy state auxilliary information */
28770 {
28771 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28772 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28773 rt_FREE(periodicStatesInfo);
28774 rt_FREE(statesInfo2);
28775 }
28776
28777 {
28778 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28779 rt_FREE(ptr);
28780 }
28781
28782 {
28783 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28784 rt_FREE(ptr);
28785 }
28786
28787 {
28788 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
28789 rt_FREE(ptr);
28790 }
28791
28792 rt_FREE(ssGetPortInfoForInputs(rts));
28793 rt_FREE(ssGetPortInfoForOutputs(rts));
28794 }
28795 }
28796
28797 /* RTW Generated Level2 S-Function Block: '<S79>/Generated S-Function' (Gain_sf) */
28798 {
28799 SimStruct *rts = ssGetSFunction(S, 115);
28800 if (rts != (NULL) ) {
28801 rt_FREE(ssGetSampleTimePtr(rts));
28802 rt_FREE(ssGetOffsetTimePtr(rts));
28803 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28804
28805 /* Destroy model methods 3 */
28806 {
28807 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28808 rt_FREE(methods3);
28809 }
28810
28811 /* Destroy model methods 2 */
28812 {
28813 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28814 rt_FREE(methods2);
28815 }
28816
28817 /* Destroy state auxilliary information */
28818 {
28819 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28820 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28821 rt_FREE(periodicStatesInfo);
28822 rt_FREE(statesInfo2);
28823 }
28824
28825 {
28826 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28827 rt_FREE(ptr);
28828 }
28829
28830 {
28831 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28832 rt_FREE(ptr);
28833 }
28834
28835 rt_FREE(ssGetPortInfoForInputs(rts));
28836 rt_FREE(ssGetPortInfoForOutputs(rts));
28837 }
28838 }
28839
28840 /* RTW Generated Level2 S-Function Block: '<S72>/Generated S-Function6' (Res180_sf) */
28841 {
28842 SimStruct *rts = ssGetSFunction(S, 116);
28843 if (rts != (NULL) ) {
28844 rt_FREE(ssGetSampleTimePtr(rts));
28845 rt_FREE(ssGetOffsetTimePtr(rts));
28846 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28847
28848 /* Destroy model methods 3 */
28849 {
28850 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28851 rt_FREE(methods3);
28852 }
28853
28854 /* Destroy model methods 2 */
28855 {
28856 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28857 rt_FREE(methods2);
28858 }
28859
28860 /* Destroy state auxilliary information */
28861 {
28862 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28863 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28864 rt_FREE(periodicStatesInfo);
28865 rt_FREE(statesInfo2);
28866 }
28867
28868 {
28869 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28870 rt_FREE(ptr);
28871 }
28872
28873 rt_FREE(ssGetPortInfoForInputs(rts));
28874 rt_FREE(ssGetPortInfoForOutputs(rts));
28875 }
28876 }
28877
28878 /* RTW Generated Level2 S-Function Block: '<S76>/Generated S-Function1' (SaturationLimiter_sf) */
28879 {
28880 SimStruct *rts = ssGetSFunction(S, 117);
28881 if (rts != (NULL) ) {
28882 rt_FREE(ssGetSampleTimePtr(rts));
28883 rt_FREE(ssGetOffsetTimePtr(rts));
28884 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28885
28886 /* Destroy model methods 3 */
28887 {
28888 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28889 rt_FREE(methods3);
28890 }
28891
28892 /* Destroy model methods 2 */
28893 {
28894 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28895 rt_FREE(methods2);
28896 }
28897
28898 /* Destroy state auxilliary information */
28899 {
28900 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28901 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28902 rt_FREE(periodicStatesInfo);
28903 rt_FREE(statesInfo2);
28904 }
28905
28906 {
28907 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28908 rt_FREE(ptr);
28909 }
28910
28911 {
28912 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28913 rt_FREE(ptr);
28914 }
28915
28916 {
28917 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
28918 rt_FREE(ptr);
28919 }
28920
28921 rt_FREE(ssGetPortInfoForInputs(rts));
28922 rt_FREE(ssGetPortInfoForOutputs(rts));
28923 }
28924 }
28925
28926 /* RTW Generated Level2 S-Function Block: '<S78>/Generated S-Function' (Gain_sf) */
28927 {
28928 SimStruct *rts = ssGetSFunction(S, 118);
28929 if (rts != (NULL) ) {
28930 rt_FREE(ssGetSampleTimePtr(rts));
28931 rt_FREE(ssGetOffsetTimePtr(rts));
28932 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28933
28934 /* Destroy model methods 3 */
28935 {
28936 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28937 rt_FREE(methods3);
28938 }
28939
28940 /* Destroy model methods 2 */
28941 {
28942 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28943 rt_FREE(methods2);
28944 }
28945
28946 /* Destroy state auxilliary information */
28947 {
28948 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28949 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28950 rt_FREE(periodicStatesInfo);
28951 rt_FREE(statesInfo2);
28952 }
28953
28954 {
28955 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28956 rt_FREE(ptr);
28957 }
28958
28959 {
28960 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
28961 rt_FREE(ptr);
28962 }
28963
28964 rt_FREE(ssGetPortInfoForInputs(rts));
28965 rt_FREE(ssGetPortInfoForOutputs(rts));
28966 }
28967 }
28968
28969 /* RTW Generated Level2 S-Function Block: '<S75>/Generated S-Function2' (LeadLagFilter_sf) */
28970 {
28971 SimStruct *rts = ssGetSFunction(S, 119);
28972 if (rts != (NULL) ) {
28973 rt_FREE(ssGetSampleTimePtr(rts));
28974 rt_FREE(ssGetOffsetTimePtr(rts));
28975 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
28976
28977 /* Destroy model methods 3 */
28978 {
28979 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
28980 rt_FREE(methods3);
28981 }
28982
28983 /* Destroy model methods 2 */
28984 {
28985 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
28986 rt_FREE(methods2);
28987 }
28988
28989 /* Destroy state auxilliary information */
28990 {
28991 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
28992 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
28993 rt_FREE(periodicStatesInfo);
28994 rt_FREE(statesInfo2);
28995 }
28996
28997 {
28998 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
28999 rt_FREE(ptr);
29000 }
29001
29002 {
29003 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29004 rt_FREE(ptr);
29005 }
29006
29007 {
29008 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29009 rt_FREE(ptr);
29010 }
29011
29012 {
29013 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
29014 rt_FREE(ptr);
29015 }
29016
29017 {
29018 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
29019 rt_FREE(ptr);
29020 }
29021
29022 {
29023 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
29024 rt_FREE(ptr);
29025 }
29026
29027 {
29028 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
29029 rt_FREE(ptr);
29030 }
29031
29032 rt_FREE(ssGetPortInfoForInputs(rts));
29033 rt_FREE(ssGetPortInfoForOutputs(rts));
29034 rt_FREE(ssGetSFcnDWork(rts));
29035 }
29036 }
29037
29038 /* RTW Generated Level2 S-Function Block: '<S74>/Generated S-Function4' (LagFilter_sf) */
29039 {
29040 SimStruct *rts = ssGetSFunction(S, 120);
29041 if (rts != (NULL) ) {
29042 rt_FREE(ssGetSampleTimePtr(rts));
29043 rt_FREE(ssGetOffsetTimePtr(rts));
29044 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29045
29046 /* Destroy model methods 3 */
29047 {
29048 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29049 rt_FREE(methods3);
29050 }
29051
29052 /* Destroy model methods 2 */
29053 {
29054 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29055 rt_FREE(methods2);
29056 }
29057
29058 /* Destroy state auxilliary information */
29059 {
29060 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29061 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29062 rt_FREE(periodicStatesInfo);
29063 rt_FREE(statesInfo2);
29064 }
29065
29066 {
29067 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29068 rt_FREE(ptr);
29069 }
29070
29071 {
29072 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29073 rt_FREE(ptr);
29074 }
29075
29076 {
29077 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29078 rt_FREE(ptr);
29079 }
29080
29081 {
29082 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
29083 rt_FREE(ptr);
29084 }
29085
29086 {
29087 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
29088 rt_FREE(ptr);
29089 }
29090
29091 {
29092 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
29093 rt_FREE(ptr);
29094 }
29095
29096 rt_FREE(ssGetPortInfoForInputs(rts));
29097 rt_FREE(ssGetPortInfoForOutputs(rts));
29098 rt_FREE(ssGetSFcnDWork(rts));
29099 }
29100 }
29101
29102 /* RTW Generated Level2 S-Function Block: '<S209>/Generated S-Function1' (SaturationLimiter_sf) */
29103 {
29104 SimStruct *rts = ssGetSFunction(S, 121);
29105 if (rts != (NULL) ) {
29106 rt_FREE(ssGetSampleTimePtr(rts));
29107 rt_FREE(ssGetOffsetTimePtr(rts));
29108 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29109
29110 /* Destroy model methods 3 */
29111 {
29112 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29113 rt_FREE(methods3);
29114 }
29115
29116 /* Destroy model methods 2 */
29117 {
29118 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29119 rt_FREE(methods2);
29120 }
29121
29122 /* Destroy state auxilliary information */
29123 {
29124 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29125 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29126 rt_FREE(periodicStatesInfo);
29127 rt_FREE(statesInfo2);
29128 }
29129
29130 {
29131 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29132 rt_FREE(ptr);
29133 }
29134
29135 {
29136 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29137 rt_FREE(ptr);
29138 }
29139
29140 {
29141 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29142 rt_FREE(ptr);
29143 }
29144
29145 rt_FREE(ssGetPortInfoForInputs(rts));
29146 rt_FREE(ssGetPortInfoForOutputs(rts));
29147 }
29148 }
29149
29150 /* RTW Generated Level2 S-Function Block: '<S208>/Generated S-Function' (RateLimiter_sf) */
29151 {
29152 SimStruct *rts = ssGetSFunction(S, 122);
29153 if (rts != (NULL) ) {
29154 rt_FREE(ssGetSampleTimePtr(rts));
29155 rt_FREE(ssGetOffsetTimePtr(rts));
29156 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29157
29158 /* Destroy model methods 3 */
29159 {
29160 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29161 rt_FREE(methods3);
29162 }
29163
29164 /* Destroy model methods 2 */
29165 {
29166 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29167 rt_FREE(methods2);
29168 }
29169
29170 /* Destroy state auxilliary information */
29171 {
29172 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29173 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29174 rt_FREE(periodicStatesInfo);
29175 rt_FREE(statesInfo2);
29176 }
29177
29178 {
29179 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29180 rt_FREE(ptr);
29181 }
29182
29183 {
29184 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29185 rt_FREE(ptr);
29186 }
29187
29188 {
29189 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29190 rt_FREE(ptr);
29191 }
29192
29193 {
29194 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
29195 rt_FREE(ptr);
29196 }
29197
29198 {
29199 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
29200 rt_FREE(ptr);
29201 }
29202
29203 rt_FREE(ssGetPortInfoForInputs(rts));
29204 rt_FREE(ssGetPortInfoForOutputs(rts));
29205 rt_FREE(ssGetSFcnDWork(rts));
29206 }
29207 }
29208
29209 /* RTW Generated Level2 S-Function Block: '<S241>/Generated S-Function4' (LagFilter_sf) */
29210 {
29211 SimStruct *rts = ssGetSFunction(S, 123);
29212 if (rts != (NULL) ) {
29213 rt_FREE(ssGetSampleTimePtr(rts));
29214 rt_FREE(ssGetOffsetTimePtr(rts));
29215 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29216
29217 /* Destroy model methods 3 */
29218 {
29219 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29220 rt_FREE(methods3);
29221 }
29222
29223 /* Destroy model methods 2 */
29224 {
29225 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29226 rt_FREE(methods2);
29227 }
29228
29229 /* Destroy state auxilliary information */
29230 {
29231 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29232 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29233 rt_FREE(periodicStatesInfo);
29234 rt_FREE(statesInfo2);
29235 }
29236
29237 {
29238 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29239 rt_FREE(ptr);
29240 }
29241
29242 {
29243 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29244 rt_FREE(ptr);
29245 }
29246
29247 {
29248 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29249 rt_FREE(ptr);
29250 }
29251
29252 {
29253 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
29254 rt_FREE(ptr);
29255 }
29256
29257 {
29258 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
29259 rt_FREE(ptr);
29260 }
29261
29262 {
29263 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
29264 rt_FREE(ptr);
29265 }
29266
29267 rt_FREE(ssGetPortInfoForInputs(rts));
29268 rt_FREE(ssGetPortInfoForOutputs(rts));
29269 rt_FREE(ssGetSFcnDWork(rts));
29270 }
29271 }
29272
29273 /* RTW Generated Level2 S-Function Block: '<S245>/Generated S-Function' (Gain_sf) */
29274 {
29275 SimStruct *rts = ssGetSFunction(S, 124);
29276 if (rts != (NULL) ) {
29277 rt_FREE(ssGetSampleTimePtr(rts));
29278 rt_FREE(ssGetOffsetTimePtr(rts));
29279 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29280
29281 /* Destroy model methods 3 */
29282 {
29283 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29284 rt_FREE(methods3);
29285 }
29286
29287 /* Destroy model methods 2 */
29288 {
29289 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29290 rt_FREE(methods2);
29291 }
29292
29293 /* Destroy state auxilliary information */
29294 {
29295 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29296 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29297 rt_FREE(periodicStatesInfo);
29298 rt_FREE(statesInfo2);
29299 }
29300
29301 {
29302 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29303 rt_FREE(ptr);
29304 }
29305
29306 {
29307 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29308 rt_FREE(ptr);
29309 }
29310
29311 rt_FREE(ssGetPortInfoForInputs(rts));
29312 rt_FREE(ssGetPortInfoForOutputs(rts));
29313 }
29314 }
29315
29316 /* RTW Generated Level2 S-Function Block: '<S246>/Generated S-Function' (Gain_sf) */
29317 {
29318 SimStruct *rts = ssGetSFunction(S, 125);
29319 if (rts != (NULL) ) {
29320 rt_FREE(ssGetSampleTimePtr(rts));
29321 rt_FREE(ssGetOffsetTimePtr(rts));
29322 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29323
29324 /* Destroy model methods 3 */
29325 {
29326 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29327 rt_FREE(methods3);
29328 }
29329
29330 /* Destroy model methods 2 */
29331 {
29332 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29333 rt_FREE(methods2);
29334 }
29335
29336 /* Destroy state auxilliary information */
29337 {
29338 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29339 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29340 rt_FREE(periodicStatesInfo);
29341 rt_FREE(statesInfo2);
29342 }
29343
29344 {
29345 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29346 rt_FREE(ptr);
29347 }
29348
29349 {
29350 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29351 rt_FREE(ptr);
29352 }
29353
29354 rt_FREE(ssGetPortInfoForInputs(rts));
29355 rt_FREE(ssGetPortInfoForOutputs(rts));
29356 }
29357 }
29358
29359 /* RTW Generated Level2 S-Function Block: '<S239>/Generated S-Function4' (LagFilter_sf) */
29360 {
29361 SimStruct *rts = ssGetSFunction(S, 126);
29362 if (rts != (NULL) ) {
29363 rt_FREE(ssGetSampleTimePtr(rts));
29364 rt_FREE(ssGetOffsetTimePtr(rts));
29365 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29366
29367 /* Destroy model methods 3 */
29368 {
29369 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29370 rt_FREE(methods3);
29371 }
29372
29373 /* Destroy model methods 2 */
29374 {
29375 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29376 rt_FREE(methods2);
29377 }
29378
29379 /* Destroy state auxilliary information */
29380 {
29381 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29382 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29383 rt_FREE(periodicStatesInfo);
29384 rt_FREE(statesInfo2);
29385 }
29386
29387 {
29388 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29389 rt_FREE(ptr);
29390 }
29391
29392 {
29393 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29394 rt_FREE(ptr);
29395 }
29396
29397 {
29398 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29399 rt_FREE(ptr);
29400 }
29401
29402 {
29403 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
29404 rt_FREE(ptr);
29405 }
29406
29407 {
29408 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
29409 rt_FREE(ptr);
29410 }
29411
29412 {
29413 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
29414 rt_FREE(ptr);
29415 }
29416
29417 rt_FREE(ssGetPortInfoForInputs(rts));
29418 rt_FREE(ssGetPortInfoForOutputs(rts));
29419 rt_FREE(ssGetSFcnDWork(rts));
29420 }
29421 }
29422
29423 /* RTW Generated Level2 S-Function Block: '<S242>/Generated S-Function' (Gain_sf) */
29424 {
29425 SimStruct *rts = ssGetSFunction(S, 127);
29426 if (rts != (NULL) ) {
29427 rt_FREE(ssGetSampleTimePtr(rts));
29428 rt_FREE(ssGetOffsetTimePtr(rts));
29429 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29430
29431 /* Destroy model methods 3 */
29432 {
29433 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29434 rt_FREE(methods3);
29435 }
29436
29437 /* Destroy model methods 2 */
29438 {
29439 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29440 rt_FREE(methods2);
29441 }
29442
29443 /* Destroy state auxilliary information */
29444 {
29445 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29446 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29447 rt_FREE(periodicStatesInfo);
29448 rt_FREE(statesInfo2);
29449 }
29450
29451 {
29452 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29453 rt_FREE(ptr);
29454 }
29455
29456 {
29457 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29458 rt_FREE(ptr);
29459 }
29460
29461 rt_FREE(ssGetPortInfoForInputs(rts));
29462 rt_FREE(ssGetPortInfoForOutputs(rts));
29463 }
29464 }
29465
29466 /* RTW Generated Level2 S-Function Block: '<S240>/Generated S-Function4' (LagFilter_sf) */
29467 {
29468 SimStruct *rts = ssGetSFunction(S, 128);
29469 if (rts != (NULL) ) {
29470 rt_FREE(ssGetSampleTimePtr(rts));
29471 rt_FREE(ssGetOffsetTimePtr(rts));
29472 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29473
29474 /* Destroy model methods 3 */
29475 {
29476 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29477 rt_FREE(methods3);
29478 }
29479
29480 /* Destroy model methods 2 */
29481 {
29482 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29483 rt_FREE(methods2);
29484 }
29485
29486 /* Destroy state auxilliary information */
29487 {
29488 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29489 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29490 rt_FREE(periodicStatesInfo);
29491 rt_FREE(statesInfo2);
29492 }
29493
29494 {
29495 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29496 rt_FREE(ptr);
29497 }
29498
29499 {
29500 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29501 rt_FREE(ptr);
29502 }
29503
29504 {
29505 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29506 rt_FREE(ptr);
29507 }
29508
29509 {
29510 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
29511 rt_FREE(ptr);
29512 }
29513
29514 {
29515 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
29516 rt_FREE(ptr);
29517 }
29518
29519 {
29520 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
29521 rt_FREE(ptr);
29522 }
29523
29524 rt_FREE(ssGetPortInfoForInputs(rts));
29525 rt_FREE(ssGetPortInfoForOutputs(rts));
29526 rt_FREE(ssGetSFcnDWork(rts));
29527 }
29528 }
29529
29530 /* RTW Generated Level2 S-Function Block: '<S238>/Generated S-Function8' (WashoutFilter_sf) */
29531 {
29532 SimStruct *rts = ssGetSFunction(S, 129);
29533 if (rts != (NULL) ) {
29534 rt_FREE(ssGetSampleTimePtr(rts));
29535 rt_FREE(ssGetOffsetTimePtr(rts));
29536 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29537
29538 /* Destroy model methods 3 */
29539 {
29540 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29541 rt_FREE(methods3);
29542 }
29543
29544 /* Destroy model methods 2 */
29545 {
29546 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29547 rt_FREE(methods2);
29548 }
29549
29550 /* Destroy state auxilliary information */
29551 {
29552 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29553 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29554 rt_FREE(periodicStatesInfo);
29555 rt_FREE(statesInfo2);
29556 }
29557
29558 {
29559 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29560 rt_FREE(ptr);
29561 }
29562
29563 {
29564 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29565 rt_FREE(ptr);
29566 }
29567
29568 {
29569 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29570 rt_FREE(ptr);
29571 }
29572
29573 {
29574 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
29575 rt_FREE(ptr);
29576 }
29577
29578 {
29579 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
29580 rt_FREE(ptr);
29581 }
29582
29583 {
29584 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
29585 rt_FREE(ptr);
29586 }
29587
29588 rt_FREE(ssGetPortInfoForInputs(rts));
29589 rt_FREE(ssGetPortInfoForOutputs(rts));
29590 rt_FREE(ssGetSFcnDWork(rts));
29591 }
29592 }
29593
29594 /* RTW Generated Level2 S-Function Block: '<S244>/Generated S-Function' (Gain_sf) */
29595 {
29596 SimStruct *rts = ssGetSFunction(S, 130);
29597 if (rts != (NULL) ) {
29598 rt_FREE(ssGetSampleTimePtr(rts));
29599 rt_FREE(ssGetOffsetTimePtr(rts));
29600 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29601
29602 /* Destroy model methods 3 */
29603 {
29604 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29605 rt_FREE(methods3);
29606 }
29607
29608 /* Destroy model methods 2 */
29609 {
29610 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29611 rt_FREE(methods2);
29612 }
29613
29614 /* Destroy state auxilliary information */
29615 {
29616 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29617 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29618 rt_FREE(periodicStatesInfo);
29619 rt_FREE(statesInfo2);
29620 }
29621
29622 {
29623 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29624 rt_FREE(ptr);
29625 }
29626
29627 {
29628 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29629 rt_FREE(ptr);
29630 }
29631
29632 rt_FREE(ssGetPortInfoForInputs(rts));
29633 rt_FREE(ssGetPortInfoForOutputs(rts));
29634 }
29635 }
29636
29637 /* RTW Generated Level2 S-Function Block: '<S243>/Generated S-Function' (Gain_sf) */
29638 {
29639 SimStruct *rts = ssGetSFunction(S, 131);
29640 if (rts != (NULL) ) {
29641 rt_FREE(ssGetSampleTimePtr(rts));
29642 rt_FREE(ssGetOffsetTimePtr(rts));
29643 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29644
29645 /* Destroy model methods 3 */
29646 {
29647 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29648 rt_FREE(methods3);
29649 }
29650
29651 /* Destroy model methods 2 */
29652 {
29653 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29654 rt_FREE(methods2);
29655 }
29656
29657 /* Destroy state auxilliary information */
29658 {
29659 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29660 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29661 rt_FREE(periodicStatesInfo);
29662 rt_FREE(statesInfo2);
29663 }
29664
29665 {
29666 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29667 rt_FREE(ptr);
29668 }
29669
29670 {
29671 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29672 rt_FREE(ptr);
29673 }
29674
29675 rt_FREE(ssGetPortInfoForInputs(rts));
29676 rt_FREE(ssGetPortInfoForOutputs(rts));
29677 }
29678 }
29679
29680 /* RTW Generated Level2 S-Function Block: '<S225>/Generated S-Function1' (SaturationLimiter_sf) */
29681 {
29682 SimStruct *rts = ssGetSFunction(S, 132);
29683 if (rts != (NULL) ) {
29684 rt_FREE(ssGetSampleTimePtr(rts));
29685 rt_FREE(ssGetOffsetTimePtr(rts));
29686 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29687
29688 /* Destroy model methods 3 */
29689 {
29690 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29691 rt_FREE(methods3);
29692 }
29693
29694 /* Destroy model methods 2 */
29695 {
29696 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29697 rt_FREE(methods2);
29698 }
29699
29700 /* Destroy state auxilliary information */
29701 {
29702 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29703 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29704 rt_FREE(periodicStatesInfo);
29705 rt_FREE(statesInfo2);
29706 }
29707
29708 {
29709 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29710 rt_FREE(ptr);
29711 }
29712
29713 {
29714 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29715 rt_FREE(ptr);
29716 }
29717
29718 {
29719 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29720 rt_FREE(ptr);
29721 }
29722
29723 rt_FREE(ssGetPortInfoForInputs(rts));
29724 rt_FREE(ssGetPortInfoForOutputs(rts));
29725 }
29726 }
29727
29728 /* RTW Generated Level2 S-Function Block: '<S223>/Generated S-Function7' (RateLimiter_sf) */
29729 {
29730 SimStruct *rts = ssGetSFunction(S, 133);
29731 if (rts != (NULL) ) {
29732 rt_FREE(ssGetSampleTimePtr(rts));
29733 rt_FREE(ssGetOffsetTimePtr(rts));
29734 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29735
29736 /* Destroy model methods 3 */
29737 {
29738 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29739 rt_FREE(methods3);
29740 }
29741
29742 /* Destroy model methods 2 */
29743 {
29744 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29745 rt_FREE(methods2);
29746 }
29747
29748 /* Destroy state auxilliary information */
29749 {
29750 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29751 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29752 rt_FREE(periodicStatesInfo);
29753 rt_FREE(statesInfo2);
29754 }
29755
29756 {
29757 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29758 rt_FREE(ptr);
29759 }
29760
29761 {
29762 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29763 rt_FREE(ptr);
29764 }
29765
29766 {
29767 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29768 rt_FREE(ptr);
29769 }
29770
29771 {
29772 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
29773 rt_FREE(ptr);
29774 }
29775
29776 {
29777 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
29778 rt_FREE(ptr);
29779 }
29780
29781 rt_FREE(ssGetPortInfoForInputs(rts));
29782 rt_FREE(ssGetPortInfoForOutputs(rts));
29783 rt_FREE(ssGetSFcnDWork(rts));
29784 }
29785 }
29786
29787 /* RTW Generated Level2 S-Function Block: '<S226>/Generated S-Function1' (SaturationLimiter_sf) */
29788 {
29789 SimStruct *rts = ssGetSFunction(S, 134);
29790 if (rts != (NULL) ) {
29791 rt_FREE(ssGetSampleTimePtr(rts));
29792 rt_FREE(ssGetOffsetTimePtr(rts));
29793 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29794
29795 /* Destroy model methods 3 */
29796 {
29797 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29798 rt_FREE(methods3);
29799 }
29800
29801 /* Destroy model methods 2 */
29802 {
29803 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29804 rt_FREE(methods2);
29805 }
29806
29807 /* Destroy state auxilliary information */
29808 {
29809 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29810 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29811 rt_FREE(periodicStatesInfo);
29812 rt_FREE(statesInfo2);
29813 }
29814
29815 {
29816 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29817 rt_FREE(ptr);
29818 }
29819
29820 {
29821 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29822 rt_FREE(ptr);
29823 }
29824
29825 {
29826 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29827 rt_FREE(ptr);
29828 }
29829
29830 rt_FREE(ssGetPortInfoForInputs(rts));
29831 rt_FREE(ssGetPortInfoForOutputs(rts));
29832 }
29833 }
29834
29835 /* RTW Generated Level2 S-Function Block: '<S224>/Generated S-Function7' (RateLimiter_sf) */
29836 {
29837 SimStruct *rts = ssGetSFunction(S, 135);
29838 if (rts != (NULL) ) {
29839 rt_FREE(ssGetSampleTimePtr(rts));
29840 rt_FREE(ssGetOffsetTimePtr(rts));
29841 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29842
29843 /* Destroy model methods 3 */
29844 {
29845 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29846 rt_FREE(methods3);
29847 }
29848
29849 /* Destroy model methods 2 */
29850 {
29851 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29852 rt_FREE(methods2);
29853 }
29854
29855 /* Destroy state auxilliary information */
29856 {
29857 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29858 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29859 rt_FREE(periodicStatesInfo);
29860 rt_FREE(statesInfo2);
29861 }
29862
29863 {
29864 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29865 rt_FREE(ptr);
29866 }
29867
29868 {
29869 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29870 rt_FREE(ptr);
29871 }
29872
29873 {
29874 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29875 rt_FREE(ptr);
29876 }
29877
29878 {
29879 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
29880 rt_FREE(ptr);
29881 }
29882
29883 {
29884 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
29885 rt_FREE(ptr);
29886 }
29887
29888 rt_FREE(ssGetPortInfoForInputs(rts));
29889 rt_FREE(ssGetPortInfoForOutputs(rts));
29890 rt_FREE(ssGetSFcnDWork(rts));
29891 }
29892 }
29893
29894 /* RTW Generated Level2 S-Function Block: '<S252>/Generated S-Function8' (WashoutFilter_sf) */
29895 {
29896 SimStruct *rts = ssGetSFunction(S, 136);
29897 if (rts != (NULL) ) {
29898 rt_FREE(ssGetSampleTimePtr(rts));
29899 rt_FREE(ssGetOffsetTimePtr(rts));
29900 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29901
29902 /* Destroy model methods 3 */
29903 {
29904 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29905 rt_FREE(methods3);
29906 }
29907
29908 /* Destroy model methods 2 */
29909 {
29910 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29911 rt_FREE(methods2);
29912 }
29913
29914 /* Destroy state auxilliary information */
29915 {
29916 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29917 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29918 rt_FREE(periodicStatesInfo);
29919 rt_FREE(statesInfo2);
29920 }
29921
29922 {
29923 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29924 rt_FREE(ptr);
29925 }
29926
29927 {
29928 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29929 rt_FREE(ptr);
29930 }
29931
29932 {
29933 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29934 rt_FREE(ptr);
29935 }
29936
29937 {
29938 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
29939 rt_FREE(ptr);
29940 }
29941
29942 {
29943 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
29944 rt_FREE(ptr);
29945 }
29946
29947 {
29948 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
29949 rt_FREE(ptr);
29950 }
29951
29952 rt_FREE(ssGetPortInfoForInputs(rts));
29953 rt_FREE(ssGetPortInfoForOutputs(rts));
29954 rt_FREE(ssGetSFcnDWork(rts));
29955 }
29956 }
29957
29958 /* RTW Generated Level2 S-Function Block: '<S253>/Generated S-Function4' (LagFilter_sf) */
29959 {
29960 SimStruct *rts = ssGetSFunction(S, 137);
29961 if (rts != (NULL) ) {
29962 rt_FREE(ssGetSampleTimePtr(rts));
29963 rt_FREE(ssGetOffsetTimePtr(rts));
29964 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
29965
29966 /* Destroy model methods 3 */
29967 {
29968 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
29969 rt_FREE(methods3);
29970 }
29971
29972 /* Destroy model methods 2 */
29973 {
29974 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
29975 rt_FREE(methods2);
29976 }
29977
29978 /* Destroy state auxilliary information */
29979 {
29980 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
29981 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
29982 rt_FREE(periodicStatesInfo);
29983 rt_FREE(statesInfo2);
29984 }
29985
29986 {
29987 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
29988 rt_FREE(ptr);
29989 }
29990
29991 {
29992 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
29993 rt_FREE(ptr);
29994 }
29995
29996 {
29997 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
29998 rt_FREE(ptr);
29999 }
30000
30001 {
30002 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
30003 rt_FREE(ptr);
30004 }
30005
30006 {
30007 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
30008 rt_FREE(ptr);
30009 }
30010
30011 {
30012 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
30013 rt_FREE(ptr);
30014 }
30015
30016 rt_FREE(ssGetPortInfoForInputs(rts));
30017 rt_FREE(ssGetPortInfoForOutputs(rts));
30018 rt_FREE(ssGetSFcnDWork(rts));
30019 }
30020 }
30021
30022 /* RTW Generated Level2 S-Function Block: '<S254>/Generated S-Function4' (LagFilter_sf) */
30023 {
30024 SimStruct *rts = ssGetSFunction(S, 138);
30025 if (rts != (NULL) ) {
30026 rt_FREE(ssGetSampleTimePtr(rts));
30027 rt_FREE(ssGetOffsetTimePtr(rts));
30028 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30029
30030 /* Destroy model methods 3 */
30031 {
30032 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30033 rt_FREE(methods3);
30034 }
30035
30036 /* Destroy model methods 2 */
30037 {
30038 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30039 rt_FREE(methods2);
30040 }
30041
30042 /* Destroy state auxilliary information */
30043 {
30044 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30045 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30046 rt_FREE(periodicStatesInfo);
30047 rt_FREE(statesInfo2);
30048 }
30049
30050 {
30051 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30052 rt_FREE(ptr);
30053 }
30054
30055 {
30056 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30057 rt_FREE(ptr);
30058 }
30059
30060 {
30061 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
30062 rt_FREE(ptr);
30063 }
30064
30065 {
30066 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
30067 rt_FREE(ptr);
30068 }
30069
30070 {
30071 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
30072 rt_FREE(ptr);
30073 }
30074
30075 {
30076 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
30077 rt_FREE(ptr);
30078 }
30079
30080 rt_FREE(ssGetPortInfoForInputs(rts));
30081 rt_FREE(ssGetPortInfoForOutputs(rts));
30082 rt_FREE(ssGetSFcnDWork(rts));
30083 }
30084 }
30085
30086 /* RTW Generated Level2 S-Function Block: '<S259>/Generated S-Function' (Gain_sf) */
30087 {
30088 SimStruct *rts = ssGetSFunction(S, 139);
30089 if (rts != (NULL) ) {
30090 rt_FREE(ssGetSampleTimePtr(rts));
30091 rt_FREE(ssGetOffsetTimePtr(rts));
30092 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30093
30094 /* Destroy model methods 3 */
30095 {
30096 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30097 rt_FREE(methods3);
30098 }
30099
30100 /* Destroy model methods 2 */
30101 {
30102 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30103 rt_FREE(methods2);
30104 }
30105
30106 /* Destroy state auxilliary information */
30107 {
30108 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30109 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30110 rt_FREE(periodicStatesInfo);
30111 rt_FREE(statesInfo2);
30112 }
30113
30114 {
30115 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30116 rt_FREE(ptr);
30117 }
30118
30119 {
30120 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30121 rt_FREE(ptr);
30122 }
30123
30124 rt_FREE(ssGetPortInfoForInputs(rts));
30125 rt_FREE(ssGetPortInfoForOutputs(rts));
30126 }
30127 }
30128
30129 /* RTW Generated Level2 S-Function Block: '<S260>/Generated S-Function' (Gain_sf) */
30130 {
30131 SimStruct *rts = ssGetSFunction(S, 140);
30132 if (rts != (NULL) ) {
30133 rt_FREE(ssGetSampleTimePtr(rts));
30134 rt_FREE(ssGetOffsetTimePtr(rts));
30135 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30136
30137 /* Destroy model methods 3 */
30138 {
30139 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30140 rt_FREE(methods3);
30141 }
30142
30143 /* Destroy model methods 2 */
30144 {
30145 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30146 rt_FREE(methods2);
30147 }
30148
30149 /* Destroy state auxilliary information */
30150 {
30151 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30152 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30153 rt_FREE(periodicStatesInfo);
30154 rt_FREE(statesInfo2);
30155 }
30156
30157 {
30158 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30159 rt_FREE(ptr);
30160 }
30161
30162 {
30163 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30164 rt_FREE(ptr);
30165 }
30166
30167 rt_FREE(ssGetPortInfoForInputs(rts));
30168 rt_FREE(ssGetPortInfoForOutputs(rts));
30169 }
30170 }
30171
30172 /* RTW Generated Level2 S-Function Block: '<S255>/Generated S-Function9' (NegativePass_sf) */
30173 {
30174 SimStruct *rts = ssGetSFunction(S, 141);
30175 if (rts != (NULL) ) {
30176 rt_FREE(ssGetSampleTimePtr(rts));
30177 rt_FREE(ssGetOffsetTimePtr(rts));
30178 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30179
30180 /* Destroy model methods 3 */
30181 {
30182 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30183 rt_FREE(methods3);
30184 }
30185
30186 /* Destroy model methods 2 */
30187 {
30188 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30189 rt_FREE(methods2);
30190 }
30191
30192 /* Destroy state auxilliary information */
30193 {
30194 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30195 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30196 rt_FREE(periodicStatesInfo);
30197 rt_FREE(statesInfo2);
30198 }
30199
30200 {
30201 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30202 rt_FREE(ptr);
30203 }
30204
30205 rt_FREE(ssGetPortInfoForInputs(rts));
30206 rt_FREE(ssGetPortInfoForOutputs(rts));
30207 }
30208 }
30209
30210 /* RTW Generated Level2 S-Function Block: '<S266>/Generated S-Function' (WashoutFilter_sf) */
30211 {
30212 SimStruct *rts = ssGetSFunction(S, 142);
30213 if (rts != (NULL) ) {
30214 rt_FREE(ssGetSampleTimePtr(rts));
30215 rt_FREE(ssGetOffsetTimePtr(rts));
30216 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30217
30218 /* Destroy model methods 3 */
30219 {
30220 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30221 rt_FREE(methods3);
30222 }
30223
30224 /* Destroy model methods 2 */
30225 {
30226 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30227 rt_FREE(methods2);
30228 }
30229
30230 /* Destroy state auxilliary information */
30231 {
30232 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30233 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30234 rt_FREE(periodicStatesInfo);
30235 rt_FREE(statesInfo2);
30236 }
30237
30238 {
30239 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30240 rt_FREE(ptr);
30241 }
30242
30243 {
30244 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30245 rt_FREE(ptr);
30246 }
30247
30248 {
30249 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
30250 rt_FREE(ptr);
30251 }
30252
30253 {
30254 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
30255 rt_FREE(ptr);
30256 }
30257
30258 {
30259 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
30260 rt_FREE(ptr);
30261 }
30262
30263 {
30264 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
30265 rt_FREE(ptr);
30266 }
30267
30268 rt_FREE(ssGetPortInfoForInputs(rts));
30269 rt_FREE(ssGetPortInfoForOutputs(rts));
30270 rt_FREE(ssGetSFcnDWork(rts));
30271 }
30272 }
30273
30274 /* RTW Generated Level2 S-Function Block: '<S268>/Generated S-Function' (Gain_sf) */
30275 {
30276 SimStruct *rts = ssGetSFunction(S, 143);
30277 if (rts != (NULL) ) {
30278 rt_FREE(ssGetSampleTimePtr(rts));
30279 rt_FREE(ssGetOffsetTimePtr(rts));
30280 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30281
30282 /* Destroy model methods 3 */
30283 {
30284 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30285 rt_FREE(methods3);
30286 }
30287
30288 /* Destroy model methods 2 */
30289 {
30290 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30291 rt_FREE(methods2);
30292 }
30293
30294 /* Destroy state auxilliary information */
30295 {
30296 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30297 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30298 rt_FREE(periodicStatesInfo);
30299 rt_FREE(statesInfo2);
30300 }
30301
30302 {
30303 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30304 rt_FREE(ptr);
30305 }
30306
30307 {
30308 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30309 rt_FREE(ptr);
30310 }
30311
30312 rt_FREE(ssGetPortInfoForInputs(rts));
30313 rt_FREE(ssGetPortInfoForOutputs(rts));
30314 }
30315 }
30316
30317 /* RTW Generated Level2 S-Function Block: '<S265>/Generated S-Function8' (WashoutFilter_sf) */
30318 {
30319 SimStruct *rts = ssGetSFunction(S, 144);
30320 if (rts != (NULL) ) {
30321 rt_FREE(ssGetSampleTimePtr(rts));
30322 rt_FREE(ssGetOffsetTimePtr(rts));
30323 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30324
30325 /* Destroy model methods 3 */
30326 {
30327 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30328 rt_FREE(methods3);
30329 }
30330
30331 /* Destroy model methods 2 */
30332 {
30333 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30334 rt_FREE(methods2);
30335 }
30336
30337 /* Destroy state auxilliary information */
30338 {
30339 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30340 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30341 rt_FREE(periodicStatesInfo);
30342 rt_FREE(statesInfo2);
30343 }
30344
30345 {
30346 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30347 rt_FREE(ptr);
30348 }
30349
30350 {
30351 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30352 rt_FREE(ptr);
30353 }
30354
30355 {
30356 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
30357 rt_FREE(ptr);
30358 }
30359
30360 {
30361 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
30362 rt_FREE(ptr);
30363 }
30364
30365 {
30366 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
30367 rt_FREE(ptr);
30368 }
30369
30370 {
30371 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
30372 rt_FREE(ptr);
30373 }
30374
30375 rt_FREE(ssGetPortInfoForInputs(rts));
30376 rt_FREE(ssGetPortInfoForOutputs(rts));
30377 rt_FREE(ssGetSFcnDWork(rts));
30378 }
30379 }
30380
30381 /* RTW Generated Level2 S-Function Block: '<S269>/Generated S-Function' (Gain_sf) */
30382 {
30383 SimStruct *rts = ssGetSFunction(S, 145);
30384 if (rts != (NULL) ) {
30385 rt_FREE(ssGetSampleTimePtr(rts));
30386 rt_FREE(ssGetOffsetTimePtr(rts));
30387 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30388
30389 /* Destroy model methods 3 */
30390 {
30391 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30392 rt_FREE(methods3);
30393 }
30394
30395 /* Destroy model methods 2 */
30396 {
30397 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30398 rt_FREE(methods2);
30399 }
30400
30401 /* Destroy state auxilliary information */
30402 {
30403 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30404 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30405 rt_FREE(periodicStatesInfo);
30406 rt_FREE(statesInfo2);
30407 }
30408
30409 {
30410 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30411 rt_FREE(ptr);
30412 }
30413
30414 {
30415 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30416 rt_FREE(ptr);
30417 }
30418
30419 rt_FREE(ssGetPortInfoForInputs(rts));
30420 rt_FREE(ssGetPortInfoForOutputs(rts));
30421 }
30422 }
30423
30424 /* RTW Generated Level2 S-Function Block: '<S267>/Generated S-Function' (NegativePass_sf) */
30425 {
30426 SimStruct *rts = ssGetSFunction(S, 146);
30427 if (rts != (NULL) ) {
30428 rt_FREE(ssGetSampleTimePtr(rts));
30429 rt_FREE(ssGetOffsetTimePtr(rts));
30430 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30431
30432 /* Destroy model methods 3 */
30433 {
30434 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30435 rt_FREE(methods3);
30436 }
30437
30438 /* Destroy model methods 2 */
30439 {
30440 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30441 rt_FREE(methods2);
30442 }
30443
30444 /* Destroy state auxilliary information */
30445 {
30446 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30447 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30448 rt_FREE(periodicStatesInfo);
30449 rt_FREE(statesInfo2);
30450 }
30451
30452 {
30453 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30454 rt_FREE(ptr);
30455 }
30456
30457 rt_FREE(ssGetPortInfoForInputs(rts));
30458 rt_FREE(ssGetPortInfoForOutputs(rts));
30459 }
30460 }
30461
30462 /* RTW Generated Level2 S-Function Block: '<S270>/Generated S-Function' (Gain_sf) */
30463 {
30464 SimStruct *rts = ssGetSFunction(S, 147);
30465 if (rts != (NULL) ) {
30466 rt_FREE(ssGetSampleTimePtr(rts));
30467 rt_FREE(ssGetOffsetTimePtr(rts));
30468 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30469
30470 /* Destroy model methods 3 */
30471 {
30472 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30473 rt_FREE(methods3);
30474 }
30475
30476 /* Destroy model methods 2 */
30477 {
30478 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30479 rt_FREE(methods2);
30480 }
30481
30482 /* Destroy state auxilliary information */
30483 {
30484 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30485 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30486 rt_FREE(periodicStatesInfo);
30487 rt_FREE(statesInfo2);
30488 }
30489
30490 {
30491 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30492 rt_FREE(ptr);
30493 }
30494
30495 {
30496 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30497 rt_FREE(ptr);
30498 }
30499
30500 rt_FREE(ssGetPortInfoForInputs(rts));
30501 rt_FREE(ssGetPortInfoForOutputs(rts));
30502 }
30503 }
30504
30505 /* RTW Generated Level2 S-Function Block: '<S272>/Generated S-Function' (PositivePass_sf) */
30506 {
30507 SimStruct *rts = ssGetSFunction(S, 148);
30508 if (rts != (NULL) ) {
30509 rt_FREE(ssGetSampleTimePtr(rts));
30510 rt_FREE(ssGetOffsetTimePtr(rts));
30511 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30512
30513 /* Destroy model methods 3 */
30514 {
30515 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30516 rt_FREE(methods3);
30517 }
30518
30519 /* Destroy model methods 2 */
30520 {
30521 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30522 rt_FREE(methods2);
30523 }
30524
30525 /* Destroy state auxilliary information */
30526 {
30527 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30528 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30529 rt_FREE(periodicStatesInfo);
30530 rt_FREE(statesInfo2);
30531 }
30532
30533 {
30534 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30535 rt_FREE(ptr);
30536 }
30537
30538 rt_FREE(ssGetPortInfoForInputs(rts));
30539 rt_FREE(ssGetPortInfoForOutputs(rts));
30540 }
30541 }
30542
30543 /* RTW Generated Level2 S-Function Block: '<S271>/Generated S-Function' (Gain_sf) */
30544 {
30545 SimStruct *rts = ssGetSFunction(S, 149);
30546 if (rts != (NULL) ) {
30547 rt_FREE(ssGetSampleTimePtr(rts));
30548 rt_FREE(ssGetOffsetTimePtr(rts));
30549 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30550
30551 /* Destroy model methods 3 */
30552 {
30553 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30554 rt_FREE(methods3);
30555 }
30556
30557 /* Destroy model methods 2 */
30558 {
30559 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30560 rt_FREE(methods2);
30561 }
30562
30563 /* Destroy state auxilliary information */
30564 {
30565 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30566 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30567 rt_FREE(periodicStatesInfo);
30568 rt_FREE(statesInfo2);
30569 }
30570
30571 {
30572 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30573 rt_FREE(ptr);
30574 }
30575
30576 {
30577 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30578 rt_FREE(ptr);
30579 }
30580
30581 rt_FREE(ssGetPortInfoForInputs(rts));
30582 rt_FREE(ssGetPortInfoForOutputs(rts));
30583 }
30584 }
30585
30586 /* RTW Generated Level2 S-Function Block: '<S261>/Generated S-Function' (Gain_sf) */
30587 {
30588 SimStruct *rts = ssGetSFunction(S, 150);
30589 if (rts != (NULL) ) {
30590 rt_FREE(ssGetSampleTimePtr(rts));
30591 rt_FREE(ssGetOffsetTimePtr(rts));
30592 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30593
30594 /* Destroy model methods 3 */
30595 {
30596 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30597 rt_FREE(methods3);
30598 }
30599
30600 /* Destroy model methods 2 */
30601 {
30602 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30603 rt_FREE(methods2);
30604 }
30605
30606 /* Destroy state auxilliary information */
30607 {
30608 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30609 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30610 rt_FREE(periodicStatesInfo);
30611 rt_FREE(statesInfo2);
30612 }
30613
30614 {
30615 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30616 rt_FREE(ptr);
30617 }
30618
30619 {
30620 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30621 rt_FREE(ptr);
30622 }
30623
30624 rt_FREE(ssGetPortInfoForInputs(rts));
30625 rt_FREE(ssGetPortInfoForOutputs(rts));
30626 }
30627 }
30628
30629 /* RTW Generated Level2 S-Function Block: '<S262>/Generated S-Function' (Gain_sf) */
30630 {
30631 SimStruct *rts = ssGetSFunction(S, 151);
30632 if (rts != (NULL) ) {
30633 rt_FREE(ssGetSampleTimePtr(rts));
30634 rt_FREE(ssGetOffsetTimePtr(rts));
30635 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30636
30637 /* Destroy model methods 3 */
30638 {
30639 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30640 rt_FREE(methods3);
30641 }
30642
30643 /* Destroy model methods 2 */
30644 {
30645 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30646 rt_FREE(methods2);
30647 }
30648
30649 /* Destroy state auxilliary information */
30650 {
30651 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30652 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30653 rt_FREE(periodicStatesInfo);
30654 rt_FREE(statesInfo2);
30655 }
30656
30657 {
30658 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30659 rt_FREE(ptr);
30660 }
30661
30662 {
30663 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30664 rt_FREE(ptr);
30665 }
30666
30667 rt_FREE(ssGetPortInfoForInputs(rts));
30668 rt_FREE(ssGetPortInfoForOutputs(rts));
30669 }
30670 }
30671
30672 /* RTW Generated Level2 S-Function Block: '<S258>/Generated S-Function1' (SaturationLimiter_sf) */
30673 {
30674 SimStruct *rts = ssGetSFunction(S, 152);
30675 if (rts != (NULL) ) {
30676 rt_FREE(ssGetSampleTimePtr(rts));
30677 rt_FREE(ssGetOffsetTimePtr(rts));
30678 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30679
30680 /* Destroy model methods 3 */
30681 {
30682 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30683 rt_FREE(methods3);
30684 }
30685
30686 /* Destroy model methods 2 */
30687 {
30688 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30689 rt_FREE(methods2);
30690 }
30691
30692 /* Destroy state auxilliary information */
30693 {
30694 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30695 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30696 rt_FREE(periodicStatesInfo);
30697 rt_FREE(statesInfo2);
30698 }
30699
30700 {
30701 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30702 rt_FREE(ptr);
30703 }
30704
30705 {
30706 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30707 rt_FREE(ptr);
30708 }
30709
30710 {
30711 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
30712 rt_FREE(ptr);
30713 }
30714
30715 rt_FREE(ssGetPortInfoForInputs(rts));
30716 rt_FREE(ssGetPortInfoForOutputs(rts));
30717 }
30718 }
30719
30720 /* RTW Generated Level2 S-Function Block: '<S263>/Generated S-Function' (Gain_sf) */
30721 {
30722 SimStruct *rts = ssGetSFunction(S, 153);
30723 if (rts != (NULL) ) {
30724 rt_FREE(ssGetSampleTimePtr(rts));
30725 rt_FREE(ssGetOffsetTimePtr(rts));
30726 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30727
30728 /* Destroy model methods 3 */
30729 {
30730 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30731 rt_FREE(methods3);
30732 }
30733
30734 /* Destroy model methods 2 */
30735 {
30736 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30737 rt_FREE(methods2);
30738 }
30739
30740 /* Destroy state auxilliary information */
30741 {
30742 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30743 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30744 rt_FREE(periodicStatesInfo);
30745 rt_FREE(statesInfo2);
30746 }
30747
30748 {
30749 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30750 rt_FREE(ptr);
30751 }
30752
30753 {
30754 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30755 rt_FREE(ptr);
30756 }
30757
30758 rt_FREE(ssGetPortInfoForInputs(rts));
30759 rt_FREE(ssGetPortInfoForOutputs(rts));
30760 }
30761 }
30762
30763 /* RTW Generated Level2 S-Function Block: '<S264>/Generated S-Function' (Gain_sf) */
30764 {
30765 SimStruct *rts = ssGetSFunction(S, 154);
30766 if (rts != (NULL) ) {
30767 rt_FREE(ssGetSampleTimePtr(rts));
30768 rt_FREE(ssGetOffsetTimePtr(rts));
30769 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30770
30771 /* Destroy model methods 3 */
30772 {
30773 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30774 rt_FREE(methods3);
30775 }
30776
30777 /* Destroy model methods 2 */
30778 {
30779 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30780 rt_FREE(methods2);
30781 }
30782
30783 /* Destroy state auxilliary information */
30784 {
30785 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30786 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30787 rt_FREE(periodicStatesInfo);
30788 rt_FREE(statesInfo2);
30789 }
30790
30791 {
30792 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30793 rt_FREE(ptr);
30794 }
30795
30796 {
30797 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30798 rt_FREE(ptr);
30799 }
30800
30801 rt_FREE(ssGetPortInfoForInputs(rts));
30802 rt_FREE(ssGetPortInfoForOutputs(rts));
30803 }
30804 }
30805
30806 /* RTW Generated Level2 S-Function Block: '<S257>/Generated S-Function3' (IntegratorLimited_sf) */
30807 {
30808 SimStruct *rts = ssGetSFunction(S, 155);
30809 if (rts != (NULL) ) {
30810 rt_FREE(ssGetSampleTimePtr(rts));
30811 rt_FREE(ssGetOffsetTimePtr(rts));
30812 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30813
30814 /* Destroy model methods 3 */
30815 {
30816 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30817 rt_FREE(methods3);
30818 }
30819
30820 /* Destroy model methods 2 */
30821 {
30822 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30823 rt_FREE(methods2);
30824 }
30825
30826 /* Destroy state auxilliary information */
30827 {
30828 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30829 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30830 rt_FREE(periodicStatesInfo);
30831 rt_FREE(statesInfo2);
30832 }
30833
30834 {
30835 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30836 rt_FREE(ptr);
30837 }
30838
30839 {
30840 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30841 rt_FREE(ptr);
30842 }
30843
30844 {
30845 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
30846 rt_FREE(ptr);
30847 }
30848
30849 {
30850 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
30851 rt_FREE(ptr);
30852 }
30853
30854 {
30855 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
30856 rt_FREE(ptr);
30857 }
30858
30859 {
30860 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
30861 rt_FREE(ptr);
30862 }
30863
30864 {
30865 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 6);
30866 rt_FREE(ptr);
30867 }
30868
30869 rt_FREE(ssGetPortInfoForInputs(rts));
30870 rt_FREE(ssGetPortInfoForOutputs(rts));
30871 rt_FREE(ssGetSFcnDWork(rts));
30872 }
30873 }
30874
30875 /* RTW Generated Level2 S-Function Block: '<S250>/Generated S-Function1' (SaturationLimiter_sf) */
30876 {
30877 SimStruct *rts = ssGetSFunction(S, 156);
30878 if (rts != (NULL) ) {
30879 rt_FREE(ssGetSampleTimePtr(rts));
30880 rt_FREE(ssGetOffsetTimePtr(rts));
30881 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30882
30883 /* Destroy model methods 3 */
30884 {
30885 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30886 rt_FREE(methods3);
30887 }
30888
30889 /* Destroy model methods 2 */
30890 {
30891 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30892 rt_FREE(methods2);
30893 }
30894
30895 /* Destroy state auxilliary information */
30896 {
30897 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30898 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30899 rt_FREE(periodicStatesInfo);
30900 rt_FREE(statesInfo2);
30901 }
30902
30903 {
30904 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30905 rt_FREE(ptr);
30906 }
30907
30908 {
30909 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30910 rt_FREE(ptr);
30911 }
30912
30913 {
30914 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
30915 rt_FREE(ptr);
30916 }
30917
30918 rt_FREE(ssGetPortInfoForInputs(rts));
30919 rt_FREE(ssGetPortInfoForOutputs(rts));
30920 }
30921 }
30922
30923 /* RTW Generated Level2 S-Function Block: '<S249>/Generated S-Function7' (RateLimiter_sf) */
30924 {
30925 SimStruct *rts = ssGetSFunction(S, 157);
30926 if (rts != (NULL) ) {
30927 rt_FREE(ssGetSampleTimePtr(rts));
30928 rt_FREE(ssGetOffsetTimePtr(rts));
30929 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30930
30931 /* Destroy model methods 3 */
30932 {
30933 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30934 rt_FREE(methods3);
30935 }
30936
30937 /* Destroy model methods 2 */
30938 {
30939 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30940 rt_FREE(methods2);
30941 }
30942
30943 /* Destroy state auxilliary information */
30944 {
30945 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
30946 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
30947 rt_FREE(periodicStatesInfo);
30948 rt_FREE(statesInfo2);
30949 }
30950
30951 {
30952 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
30953 rt_FREE(ptr);
30954 }
30955
30956 {
30957 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
30958 rt_FREE(ptr);
30959 }
30960
30961 {
30962 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
30963 rt_FREE(ptr);
30964 }
30965
30966 {
30967 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
30968 rt_FREE(ptr);
30969 }
30970
30971 {
30972 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
30973 rt_FREE(ptr);
30974 }
30975
30976 rt_FREE(ssGetPortInfoForInputs(rts));
30977 rt_FREE(ssGetPortInfoForOutputs(rts));
30978 rt_FREE(ssGetSFcnDWork(rts));
30979 }
30980 }
30981
30982 /* RTW Generated Level2 S-Function Block: '<S275>/Generated S-Function1' (SaturationLimiter_sf) */
30983 {
30984 SimStruct *rts = ssGetSFunction(S, 158);
30985 if (rts != (NULL) ) {
30986 rt_FREE(ssGetSampleTimePtr(rts));
30987 rt_FREE(ssGetOffsetTimePtr(rts));
30988 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
30989
30990 /* Destroy model methods 3 */
30991 {
30992 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
30993 rt_FREE(methods3);
30994 }
30995
30996 /* Destroy model methods 2 */
30997 {
30998 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
30999 rt_FREE(methods2);
31000 }
31001
31002 /* Destroy state auxilliary information */
31003 {
31004 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
31005 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
31006 rt_FREE(periodicStatesInfo);
31007 rt_FREE(statesInfo2);
31008 }
31009
31010 {
31011 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
31012 rt_FREE(ptr);
31013 }
31014
31015 {
31016 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
31017 rt_FREE(ptr);
31018 }
31019
31020 {
31021 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
31022 rt_FREE(ptr);
31023 }
31024
31025 rt_FREE(ssGetPortInfoForInputs(rts));
31026 rt_FREE(ssGetPortInfoForOutputs(rts));
31027 }
31028 }
31029
31030 /* RTW Generated Level2 S-Function Block: '<S274>/Generated S-Function7' (RateLimiter_sf) */
31031 {
31032 SimStruct *rts = ssGetSFunction(S, 159);
31033 if (rts != (NULL) ) {
31034 rt_FREE(ssGetSampleTimePtr(rts));
31035 rt_FREE(ssGetOffsetTimePtr(rts));
31036 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
31037
31038 /* Destroy model methods 3 */
31039 {
31040 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
31041 rt_FREE(methods3);
31042 }
31043
31044 /* Destroy model methods 2 */
31045 {
31046 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
31047 rt_FREE(methods2);
31048 }
31049
31050 /* Destroy state auxilliary information */
31051 {
31052 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
31053 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
31054 rt_FREE(periodicStatesInfo);
31055 rt_FREE(statesInfo2);
31056 }
31057
31058 {
31059 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
31060 rt_FREE(ptr);
31061 }
31062
31063 {
31064 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
31065 rt_FREE(ptr);
31066 }
31067
31068 {
31069 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
31070 rt_FREE(ptr);
31071 }
31072
31073 {
31074 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
31075 rt_FREE(ptr);
31076 }
31077
31078 {
31079 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
31080 rt_FREE(ptr);
31081 }
31082
31083 rt_FREE(ssGetPortInfoForInputs(rts));
31084 rt_FREE(ssGetPortInfoForOutputs(rts));
31085 rt_FREE(ssGetSFcnDWork(rts));
31086 }
31087 }
31088
31089 /* RTW Generated Level2 S-Function Block: '<S215>/Generated S-Function4' (LagFilter_sf) */
31090 {
31091 SimStruct *rts = ssGetSFunction(S, 160);
31092 if (rts != (NULL) ) {
31093 rt_FREE(ssGetSampleTimePtr(rts));
31094 rt_FREE(ssGetOffsetTimePtr(rts));
31095 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
31096
31097 /* Destroy model methods 3 */
31098 {
31099 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
31100 rt_FREE(methods3);
31101 }
31102
31103 /* Destroy model methods 2 */
31104 {
31105 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
31106 rt_FREE(methods2);
31107 }
31108
31109 /* Destroy state auxilliary information */
31110 {
31111 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
31112 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
31113 rt_FREE(periodicStatesInfo);
31114 rt_FREE(statesInfo2);
31115 }
31116
31117 {
31118 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
31119 rt_FREE(ptr);
31120 }
31121
31122 {
31123 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
31124 rt_FREE(ptr);
31125 }
31126
31127 {
31128 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
31129 rt_FREE(ptr);
31130 }
31131
31132 {
31133 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
31134 rt_FREE(ptr);
31135 }
31136
31137 {
31138 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
31139 rt_FREE(ptr);
31140 }
31141
31142 {
31143 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 5);
31144 rt_FREE(ptr);
31145 }
31146
31147 rt_FREE(ssGetPortInfoForInputs(rts));
31148 rt_FREE(ssGetPortInfoForOutputs(rts));
31149 rt_FREE(ssGetSFcnDWork(rts));
31150 }
31151 }
31152
31153 /* RTW Generated Level2 S-Function Block: '<S218>/Generated S-Function' (Gain_sf) */
31154 {
31155 SimStruct *rts = ssGetSFunction(S, 161);
31156 if (rts != (NULL) ) {
31157 rt_FREE(ssGetSampleTimePtr(rts));
31158 rt_FREE(ssGetOffsetTimePtr(rts));
31159 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
31160
31161 /* Destroy model methods 3 */
31162 {
31163 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
31164 rt_FREE(methods3);
31165 }
31166
31167 /* Destroy model methods 2 */
31168 {
31169 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
31170 rt_FREE(methods2);
31171 }
31172
31173 /* Destroy state auxilliary information */
31174 {
31175 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
31176 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
31177 rt_FREE(periodicStatesInfo);
31178 rt_FREE(statesInfo2);
31179 }
31180
31181 {
31182 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
31183 rt_FREE(ptr);
31184 }
31185
31186 {
31187 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
31188 rt_FREE(ptr);
31189 }
31190
31191 rt_FREE(ssGetPortInfoForInputs(rts));
31192 rt_FREE(ssGetPortInfoForOutputs(rts));
31193 }
31194 }
31195
31196 /* RTW Generated Level2 S-Function Block: '<S217>/Generated S-Function1' (SaturationLimiter_sf) */
31197 {
31198 SimStruct *rts = ssGetSFunction(S, 162);
31199 if (rts != (NULL) ) {
31200 rt_FREE(ssGetSampleTimePtr(rts));
31201 rt_FREE(ssGetOffsetTimePtr(rts));
31202 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
31203
31204 /* Destroy model methods 3 */
31205 {
31206 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
31207 rt_FREE(methods3);
31208 }
31209
31210 /* Destroy model methods 2 */
31211 {
31212 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
31213 rt_FREE(methods2);
31214 }
31215
31216 /* Destroy state auxilliary information */
31217 {
31218 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
31219 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
31220 rt_FREE(periodicStatesInfo);
31221 rt_FREE(statesInfo2);
31222 }
31223
31224 {
31225 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
31226 rt_FREE(ptr);
31227 }
31228
31229 {
31230 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
31231 rt_FREE(ptr);
31232 }
31233
31234 {
31235 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
31236 rt_FREE(ptr);
31237 }
31238
31239 rt_FREE(ssGetPortInfoForInputs(rts));
31240 rt_FREE(ssGetPortInfoForOutputs(rts));
31241 }
31242 }
31243
31244 /* RTW Generated Level2 S-Function Block: '<S216>/Generated S-Function7' (RateLimiter_sf) */
31245 {
31246 SimStruct *rts = ssGetSFunction(S, 163);
31247 if (rts != (NULL) ) {
31248 rt_FREE(ssGetSampleTimePtr(rts));
31249 rt_FREE(ssGetOffsetTimePtr(rts));
31250 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
31251
31252 /* Destroy model methods 3 */
31253 {
31254 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
31255 rt_FREE(methods3);
31256 }
31257
31258 /* Destroy model methods 2 */
31259 {
31260 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
31261 rt_FREE(methods2);
31262 }
31263
31264 /* Destroy state auxilliary information */
31265 {
31266 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
31267 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
31268 rt_FREE(periodicStatesInfo);
31269 rt_FREE(statesInfo2);
31270 }
31271
31272 {
31273 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
31274 rt_FREE(ptr);
31275 }
31276
31277 {
31278 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
31279 rt_FREE(ptr);
31280 }
31281
31282 {
31283 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
31284 rt_FREE(ptr);
31285 }
31286
31287 {
31288 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
31289 rt_FREE(ptr);
31290 }
31291
31292 {
31293 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 4);
31294 rt_FREE(ptr);
31295 }
31296
31297 rt_FREE(ssGetPortInfoForInputs(rts));
31298 rt_FREE(ssGetPortInfoForOutputs(rts));
31299 rt_FREE(ssGetSFcnDWork(rts));
31300 }
31301 }
31302
31303 /* RTW Generated Level2 S-Function Block: '<S279>/Generated S-Function' (Gain_sf) */
31304 {
31305 SimStruct *rts = ssGetSFunction(S, 164);
31306 if (rts != (NULL) ) {
31307 rt_FREE(ssGetSampleTimePtr(rts));
31308 rt_FREE(ssGetOffsetTimePtr(rts));
31309 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
31310
31311 /* Destroy model methods 3 */
31312 {
31313 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
31314 rt_FREE(methods3);
31315 }
31316
31317 /* Destroy model methods 2 */
31318 {
31319 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
31320 rt_FREE(methods2);
31321 }
31322
31323 /* Destroy state auxilliary information */
31324 {
31325 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
31326 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
31327 rt_FREE(periodicStatesInfo);
31328 rt_FREE(statesInfo2);
31329 }
31330
31331 {
31332 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
31333 rt_FREE(ptr);
31334 }
31335
31336 {
31337 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
31338 rt_FREE(ptr);
31339 }
31340
31341 rt_FREE(ssGetPortInfoForInputs(rts));
31342 rt_FREE(ssGetPortInfoForOutputs(rts));
31343 }
31344 }
31345
31346 /* RTW Generated Level2 S-Function Block: '<S280>/Generated S-Function' (Gain_sf) */
31347 {
31348 SimStruct *rts = ssGetSFunction(S, 165);
31349 if (rts != (NULL) ) {
31350 rt_FREE(ssGetSampleTimePtr(rts));
31351 rt_FREE(ssGetOffsetTimePtr(rts));
31352 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
31353
31354 /* Destroy model methods 3 */
31355 {
31356 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
31357 rt_FREE(methods3);
31358 }
31359
31360 /* Destroy model methods 2 */
31361 {
31362 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
31363 rt_FREE(methods2);
31364 }
31365
31366 /* Destroy state auxilliary information */
31367 {
31368 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
31369 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
31370 rt_FREE(periodicStatesInfo);
31371 rt_FREE(statesInfo2);
31372 }
31373
31374 {
31375 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
31376 rt_FREE(ptr);
31377 }
31378
31379 {
31380 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
31381 rt_FREE(ptr);
31382 }
31383
31384 rt_FREE(ssGetPortInfoForInputs(rts));
31385 rt_FREE(ssGetPortInfoForOutputs(rts));
31386 }
31387 }
31388
31389 /* RTW Generated Level2 S-Function Block: '<S281>/Generated S-Function' (Gain_sf) */
31390 {
31391 SimStruct *rts = ssGetSFunction(S, 166);
31392 if (rts != (NULL) ) {
31393 rt_FREE(ssGetSampleTimePtr(rts));
31394 rt_FREE(ssGetOffsetTimePtr(rts));
31395 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
31396
31397 /* Destroy model methods 3 */
31398 {
31399 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
31400 rt_FREE(methods3);
31401 }
31402
31403 /* Destroy model methods 2 */
31404 {
31405 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
31406 rt_FREE(methods2);
31407 }
31408
31409 /* Destroy state auxilliary information */
31410 {
31411 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
31412 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
31413 rt_FREE(periodicStatesInfo);
31414 rt_FREE(statesInfo2);
31415 }
31416
31417 {
31418 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
31419 rt_FREE(ptr);
31420 }
31421
31422 {
31423 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
31424 rt_FREE(ptr);
31425 }
31426
31427 rt_FREE(ssGetPortInfoForInputs(rts));
31428 rt_FREE(ssGetPortInfoForOutputs(rts));
31429 }
31430 }
31431
31432 /* RTW Generated Level2 S-Function Block: '<S306>/Generated S-Function15' (StickGradientDeadZone_sf) */
31433 {
31434 SimStruct *rts = ssGetSFunction(S, 167);
31435 if (rts != (NULL) ) {
31436 rt_FREE(ssGetSampleTimePtr(rts));
31437 rt_FREE(ssGetOffsetTimePtr(rts));
31438 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
31439
31440 /* Destroy model methods 3 */
31441 {
31442 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
31443 rt_FREE(methods3);
31444 }
31445
31446 /* Destroy model methods 2 */
31447 {
31448 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
31449 rt_FREE(methods2);
31450 }
31451
31452 /* Destroy state auxilliary information */
31453 {
31454 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
31455 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
31456 rt_FREE(periodicStatesInfo);
31457 rt_FREE(statesInfo2);
31458 }
31459
31460 {
31461 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
31462 rt_FREE(ptr);
31463 }
31464
31465 {
31466 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
31467 rt_FREE(ptr);
31468 }
31469
31470 {
31471 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
31472 rt_FREE(ptr);
31473 }
31474
31475 {
31476 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
31477 rt_FREE(ptr);
31478 }
31479
31480 rt_FREE(ssGetPortInfoForInputs(rts));
31481 rt_FREE(ssGetPortInfoForOutputs(rts));
31482 }
31483 }
31484
31485 /* RTW Generated Level2 S-Function Block: '<S305>/Generated S-Function15' (StickGradientDeadZone_sf) */
31486 {
31487 SimStruct *rts = ssGetSFunction(S, 168);
31488 if (rts != (NULL) ) {
31489 rt_FREE(ssGetSampleTimePtr(rts));
31490 rt_FREE(ssGetOffsetTimePtr(rts));
31491 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
31492
31493 /* Destroy model methods 3 */
31494 {
31495 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
31496 rt_FREE(methods3);
31497 }
31498
31499 /* Destroy model methods 2 */
31500 {
31501 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
31502 rt_FREE(methods2);
31503 }
31504
31505 /* Destroy state auxilliary information */
31506 {
31507 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
31508 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
31509 rt_FREE(periodicStatesInfo);
31510 rt_FREE(statesInfo2);
31511 }
31512
31513 {
31514 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
31515 rt_FREE(ptr);
31516 }
31517
31518 {
31519 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
31520 rt_FREE(ptr);
31521 }
31522
31523 {
31524 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
31525 rt_FREE(ptr);
31526 }
31527
31528 {
31529 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
31530 rt_FREE(ptr);
31531 }
31532
31533 rt_FREE(ssGetPortInfoForInputs(rts));
31534 rt_FREE(ssGetPortInfoForOutputs(rts));
31535 }
31536 }
31537
31538 /* RTW Generated Level2 S-Function Block: '<S304>/Generated S-Function15' (StickGradientDeadZone_sf) */
31539 {
31540 SimStruct *rts = ssGetSFunction(S, 169);
31541 if (rts != (NULL) ) {
31542 rt_FREE(ssGetSampleTimePtr(rts));
31543 rt_FREE(ssGetOffsetTimePtr(rts));
31544 rt_FREE(ssGetSampleTimeTaskIDPtr(rts));
31545
31546 /* Destroy model methods 3 */
31547 {
31548 struct _ssSFcnModelMethods3 *methods3 = ssGetModelMethods3(rts);
31549 rt_FREE(methods3);
31550 }
31551
31552 /* Destroy model methods 2 */
31553 {
31554 struct _ssSFcnModelMethods2 *methods2 = ssGetModelMethods2(rts);
31555 rt_FREE(methods2);
31556 }
31557
31558 /* Destroy state auxilliary information */
31559 {
31560 struct _ssStatesInfo2 *statesInfo2 = ssGetStatesInfo2(rts);
31561 ssPeriodicStatesInfo* periodicStatesInfo = ssGetPeriodicStatesInfo(rts);
31562 rt_FREE(periodicStatesInfo);
31563 rt_FREE(statesInfo2);
31564 }
31565
31566 {
31567 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 0);
31568 rt_FREE(ptr);
31569 }
31570
31571 {
31572 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 1);
31573 rt_FREE(ptr);
31574 }
31575
31576 {
31577 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 2);
31578 rt_FREE(ptr);
31579 }
31580
31581 {
31582 void **ptr = (void**) ssGetInputPortSignalPtrs(rts, 3);
31583 rt_FREE(ptr);
31584 }
31585
31586 rt_FREE(ssGetPortInfoForInputs(rts));
31587 rt_FREE(ssGetPortInfoForOutputs(rts));
31588 }
31589 }
31590
31591 rt_FREE(ssGetSFunction(S, 0))
31592 rt_FREE(_ssGetSFunctions(S));
31593 }
31594
31595 if (ssGetUserData(S) != (NULL) ) {
31596 rt_FREE(ssGetLocalBlockIO(S));
31597 rt_FREE(_ssGetPrevZCSigState(S));
31598 }
31599
31600 rt_FREE(ssGetUserData(S));
31601
31602#endif
31603
31604}
31605
31606#if defined(RT_MALLOC) || defined(MATLAB_MEX_FILE)
31607# include "AFCS_MODEL1_mid.h"
31608#endif
31609
31610/* Function to initialize sizes. */
31611static void mdlInitializeSizes(SimStruct *S)
31612{
31613 ssSetNumSampleTimes(S, 2); /* Number of sample times */
31614 ssSetNumContStates(S, 39); /* Number of continuous states */
31615 ssSetNumPeriodicContStates(S, 0); /* Number of periodic continuous states */
31616 ssSetNumNonsampledZCs(S, 0); /* Number of nonsampled ZCs */
31617
31618 /* Number of output ports */
31619 if (!ssSetNumOutputPorts(S, 33))
31620 return;
31621
31622 /* outport number: 0 */
31623 if (!ssSetOutputPortVectorDimension(S, 0, 1))
31624 return;
31625 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31626 ssSetOutputPortDataType(S, 0, SS_DOUBLE);
31627 }
31628
31629 ssSetOutputPortSampleTime(S, 0, 0.0);
31630 ssSetOutputPortOffsetTime(S, 0, 0.0);
31631 ssSetOutputPortOptimOpts(S, 0, SS_REUSABLE_AND_LOCAL);
31632
31633 /* outport number: 1 */
31634 if (!ssSetOutputPortVectorDimension(S, 1, 1))
31635 return;
31636 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31637 ssSetOutputPortDataType(S, 1, SS_DOUBLE);
31638 }
31639
31640 ssSetOutputPortSampleTime(S, 1, 0.0);
31641 ssSetOutputPortOffsetTime(S, 1, 0.0);
31642 ssSetOutputPortOptimOpts(S, 1, SS_REUSABLE_AND_LOCAL);
31643
31644 /* outport number: 2 */
31645 if (!ssSetOutputPortVectorDimension(S, 2, 1))
31646 return;
31647 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31648 ssSetOutputPortDataType(S, 2, SS_SINGLE);
31649 }
31650
31651 ssSetOutputPortSampleTime(S, 2, 0.0);
31652 ssSetOutputPortOffsetTime(S, 2, 0.0);
31653 ssSetOutputPortOptimOpts(S, 2, SS_REUSABLE_AND_LOCAL);
31654
31655 /* outport number: 3 */
31656 if (!ssSetOutputPortVectorDimension(S, 3, 1))
31657 return;
31658 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31659 ssSetOutputPortDataType(S, 3, SS_SINGLE);
31660 }
31661
31662 ssSetOutputPortSampleTime(S, 3, 0.0);
31663 ssSetOutputPortOffsetTime(S, 3, 0.0);
31664 ssSetOutputPortOptimOpts(S, 3, SS_REUSABLE_AND_LOCAL);
31665
31666 /* outport number: 4 */
31667 if (!ssSetOutputPortVectorDimension(S, 4, 1))
31668 return;
31669 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31670 ssSetOutputPortDataType(S, 4, SS_SINGLE);
31671 }
31672
31673 ssSetOutputPortSampleTime(S, 4, 0.0);
31674 ssSetOutputPortOffsetTime(S, 4, 0.0);
31675 ssSetOutputPortOptimOpts(S, 4, SS_REUSABLE_AND_LOCAL);
31676
31677 /* outport number: 5 */
31678 if (!ssSetOutputPortVectorDimension(S, 5, 1))
31679 return;
31680 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31681 ssSetOutputPortDataType(S, 5, SS_SINGLE);
31682 }
31683
31684 ssSetOutputPortSampleTime(S, 5, 0.0);
31685 ssSetOutputPortOffsetTime(S, 5, 0.0);
31686 ssSetOutputPortOptimOpts(S, 5, SS_REUSABLE_AND_LOCAL);
31687
31688 /* outport number: 6 */
31689 if (!ssSetOutputPortVectorDimension(S, 6, 1))
31690 return;
31691 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31692 ssSetOutputPortDataType(S, 6, SS_SINGLE);
31693 }
31694
31695 ssSetOutputPortSampleTime(S, 6, 0.0);
31696 ssSetOutputPortOffsetTime(S, 6, 0.0);
31697 ssSetOutputPortOptimOpts(S, 6, SS_REUSABLE_AND_LOCAL);
31698
31699 /* outport number: 7 */
31700 if (!ssSetOutputPortVectorDimension(S, 7, 1))
31701 return;
31702 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31703 ssSetOutputPortDataType(S, 7, SS_SINGLE);
31704 }
31705
31706 ssSetOutputPortSampleTime(S, 7, 0.0);
31707 ssSetOutputPortOffsetTime(S, 7, 0.0);
31708 ssSetOutputPortOptimOpts(S, 7, SS_REUSABLE_AND_LOCAL);
31709
31710 /* outport number: 8 */
31711 if (!ssSetOutputPortVectorDimension(S, 8, 1))
31712 return;
31713 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31714 ssSetOutputPortDataType(S, 8, SS_SINGLE);
31715 }
31716
31717 ssSetOutputPortSampleTime(S, 8, 0.0);
31718 ssSetOutputPortOffsetTime(S, 8, 0.0);
31719 ssSetOutputPortOptimOpts(S, 8, SS_REUSABLE_AND_LOCAL);
31720
31721 /* outport number: 9 */
31722 if (!ssSetOutputPortVectorDimension(S, 9, 1))
31723 return;
31724 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31725 ssSetOutputPortDataType(S, 9, SS_SINGLE);
31726 }
31727
31728 ssSetOutputPortSampleTime(S, 9, 0.0);
31729 ssSetOutputPortOffsetTime(S, 9, 0.0);
31730 ssSetOutputPortOptimOpts(S, 9, SS_REUSABLE_AND_LOCAL);
31731
31732 /* outport number: 10 */
31733 if (!ssSetOutputPortVectorDimension(S, 10, 1))
31734 return;
31735 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31736 ssSetOutputPortDataType(S, 10, SS_SINGLE);
31737 }
31738
31739 ssSetOutputPortSampleTime(S, 10, 0.0);
31740 ssSetOutputPortOffsetTime(S, 10, 0.0);
31741 ssSetOutputPortOptimOpts(S, 10, SS_REUSABLE_AND_LOCAL);
31742
31743 /* outport number: 11 */
31744 if (!ssSetOutputPortVectorDimension(S, 11, 1))
31745 return;
31746 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31747 ssSetOutputPortDataType(S, 11, SS_SINGLE);
31748 }
31749
31750 ssSetOutputPortSampleTime(S, 11, 0.0);
31751 ssSetOutputPortOffsetTime(S, 11, 0.0);
31752 ssSetOutputPortOptimOpts(S, 11, SS_REUSABLE_AND_LOCAL);
31753
31754 /* outport number: 12 */
31755 if (!ssSetOutputPortVectorDimension(S, 12, 1))
31756 return;
31757 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31758 ssSetOutputPortDataType(S, 12, SS_SINGLE);
31759 }
31760
31761 ssSetOutputPortSampleTime(S, 12, 0.0);
31762 ssSetOutputPortOffsetTime(S, 12, 0.0);
31763 ssSetOutputPortOptimOpts(S, 12, SS_REUSABLE_AND_LOCAL);
31764
31765 /* outport number: 13 */
31766 if (!ssSetOutputPortVectorDimension(S, 13, 1))
31767 return;
31768 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31769 ssSetOutputPortDataType(S, 13, SS_SINGLE);
31770 }
31771
31772 ssSetOutputPortSampleTime(S, 13, 0.0);
31773 ssSetOutputPortOffsetTime(S, 13, 0.0);
31774 ssSetOutputPortOptimOpts(S, 13, SS_REUSABLE_AND_LOCAL);
31775
31776 /* outport number: 14 */
31777 if (!ssSetOutputPortVectorDimension(S, 14, 1))
31778 return;
31779 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31780 ssSetOutputPortDataType(S, 14, SS_SINGLE);
31781 }
31782
31783 ssSetOutputPortSampleTime(S, 14, 0.0);
31784 ssSetOutputPortOffsetTime(S, 14, 0.0);
31785 ssSetOutputPortOptimOpts(S, 14, SS_REUSABLE_AND_LOCAL);
31786
31787 /* outport number: 15 */
31788 if (!ssSetOutputPortVectorDimension(S, 15, 1))
31789 return;
31790 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31791 ssSetOutputPortDataType(S, 15, SS_SINGLE);
31792 }
31793
31794 ssSetOutputPortSampleTime(S, 15, 0.0);
31795 ssSetOutputPortOffsetTime(S, 15, 0.0);
31796 ssSetOutputPortOptimOpts(S, 15, SS_REUSABLE_AND_LOCAL);
31797
31798 /* outport number: 16 */
31799 if (!ssSetOutputPortVectorDimension(S, 16, 1))
31800 return;
31801 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31802 ssSetOutputPortDataType(S, 16, SS_SINGLE);
31803 }
31804
31805 ssSetOutputPortSampleTime(S, 16, 0.0);
31806 ssSetOutputPortOffsetTime(S, 16, 0.0);
31807 ssSetOutputPortOptimOpts(S, 16, SS_REUSABLE_AND_LOCAL);
31808
31809 /* outport number: 17 */
31810 if (!ssSetOutputPortMatrixDimensions(S, 17, 1, 1))
31811 return;
31812 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31813 ssSetOutputPortDataType(S, 17, SS_SINGLE);
31814 }
31815
31816 ssSetOutputPortSampleTime(S, 17, 0.0);
31817 ssSetOutputPortOffsetTime(S, 17, 0.0);
31818 ssSetOutputPortOptimOpts(S, 17, SS_REUSABLE_AND_LOCAL);
31819
31820 /* outport number: 18 */
31821 if (!ssSetOutputPortVectorDimension(S, 18, 1))
31822 return;
31823 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31824 ssSetOutputPortDataType(S, 18, SS_SINGLE);
31825 }
31826
31827 ssSetOutputPortSampleTime(S, 18, 0.0);
31828 ssSetOutputPortOffsetTime(S, 18, 0.0);
31829 ssSetOutputPortOptimOpts(S, 18, SS_REUSABLE_AND_LOCAL);
31830
31831 /* outport number: 19 */
31832 if (!ssSetOutputPortVectorDimension(S, 19, 1))
31833 return;
31834 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31835 ssSetOutputPortDataType(S, 19, SS_SINGLE);
31836 }
31837
31838 ssSetOutputPortSampleTime(S, 19, 0.0);
31839 ssSetOutputPortOffsetTime(S, 19, 0.0);
31840 ssSetOutputPortOptimOpts(S, 19, SS_REUSABLE_AND_LOCAL);
31841
31842 /* outport number: 20 */
31843 if (!ssSetOutputPortVectorDimension(S, 20, 1))
31844 return;
31845 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31846 ssSetOutputPortDataType(S, 20, SS_DOUBLE);
31847 }
31848
31849 ssSetOutputPortSampleTime(S, 20, rtInf);
31850 ssSetOutputPortOffsetTime(S, 20, 0.0);
31851 ssSetOutputPortOptimOpts(S, 20, SS_NOT_REUSABLE_AND_LOCAL);
31852
31853 /* outport number: 21 */
31854 if (!ssSetOutputPortVectorDimension(S, 21, 1))
31855 return;
31856 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31857 ssSetOutputPortDataType(S, 21, SS_DOUBLE);
31858 }
31859
31860 ssSetOutputPortSampleTime(S, 21, rtInf);
31861 ssSetOutputPortOffsetTime(S, 21, 0.0);
31862 ssSetOutputPortOptimOpts(S, 21, SS_NOT_REUSABLE_AND_LOCAL);
31863
31864 /* outport number: 22 */
31865 if (!ssSetOutputPortVectorDimension(S, 22, 1))
31866 return;
31867 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31868 ssSetOutputPortDataType(S, 22, SS_DOUBLE);
31869 }
31870
31871 ssSetOutputPortSampleTime(S, 22, rtInf);
31872 ssSetOutputPortOffsetTime(S, 22, 0.0);
31873 ssSetOutputPortOptimOpts(S, 22, SS_NOT_REUSABLE_AND_LOCAL);
31874
31875 /* outport number: 23 */
31876 if (!ssSetOutputPortVectorDimension(S, 23, 1))
31877 return;
31878 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31879 ssSetOutputPortDataType(S, 23, SS_DOUBLE);
31880 }
31881
31882 ssSetOutputPortSampleTime(S, 23, rtInf);
31883 ssSetOutputPortOffsetTime(S, 23, 0.0);
31884 ssSetOutputPortOptimOpts(S, 23, SS_NOT_REUSABLE_AND_LOCAL);
31885
31886 /* outport number: 24 */
31887 if (!ssSetOutputPortVectorDimension(S, 24, 1))
31888 return;
31889 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31890 ssSetOutputPortDataType(S, 24, SS_SINGLE);
31891 }
31892
31893 ssSetOutputPortSampleTime(S, 24, 0.0);
31894 ssSetOutputPortOffsetTime(S, 24, 0.0);
31895 ssSetOutputPortOptimOpts(S, 24, SS_REUSABLE_AND_LOCAL);
31896
31897 /* outport number: 25 */
31898 if (!ssSetOutputPortVectorDimension(S, 25, 1))
31899 return;
31900 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31901 ssSetOutputPortDataType(S, 25, SS_SINGLE);
31902 }
31903
31904 ssSetOutputPortSampleTime(S, 25, 0.0);
31905 ssSetOutputPortOffsetTime(S, 25, 0.0);
31906 ssSetOutputPortOptimOpts(S, 25, SS_REUSABLE_AND_LOCAL);
31907
31908 /* outport number: 26 */
31909 if (!ssSetOutputPortVectorDimension(S, 26, 1))
31910 return;
31911 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31912 ssSetOutputPortDataType(S, 26, SS_SINGLE);
31913 }
31914
31915 ssSetOutputPortSampleTime(S, 26, 0.0);
31916 ssSetOutputPortOffsetTime(S, 26, 0.0);
31917 ssSetOutputPortOptimOpts(S, 26, SS_REUSABLE_AND_LOCAL);
31918
31919 /* outport number: 27 */
31920 if (!ssSetOutputPortVectorDimension(S, 27, 1))
31921 return;
31922 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31923 ssSetOutputPortDataType(S, 27, SS_SINGLE);
31924 }
31925
31926 ssSetOutputPortSampleTime(S, 27, 0.0);
31927 ssSetOutputPortOffsetTime(S, 27, 0.0);
31928 ssSetOutputPortOptimOpts(S, 27, SS_REUSABLE_AND_LOCAL);
31929
31930 /* outport number: 28 */
31931 if (!ssSetOutputPortVectorDimension(S, 28, 1))
31932 return;
31933 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31934 ssSetOutputPortDataType(S, 28, SS_SINGLE);
31935 }
31936
31937 ssSetOutputPortSampleTime(S, 28, 0.0);
31938 ssSetOutputPortOffsetTime(S, 28, 0.0);
31939 ssSetOutputPortOptimOpts(S, 28, SS_REUSABLE_AND_LOCAL);
31940
31941 /* outport number: 29 */
31942 if (!ssSetOutputPortVectorDimension(S, 29, 1))
31943 return;
31944 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31945 ssSetOutputPortDataType(S, 29, SS_SINGLE);
31946 }
31947
31948 ssSetOutputPortSampleTime(S, 29, 0.0);
31949 ssSetOutputPortOffsetTime(S, 29, 0.0);
31950 ssSetOutputPortOptimOpts(S, 29, SS_REUSABLE_AND_LOCAL);
31951
31952 /* outport number: 30 */
31953 if (!ssSetOutputPortVectorDimension(S, 30, 1))
31954 return;
31955 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31956 ssSetOutputPortDataType(S, 30, SS_SINGLE);
31957 }
31958
31959 ssSetOutputPortSampleTime(S, 30, 0.0);
31960 ssSetOutputPortOffsetTime(S, 30, 0.0);
31961 ssSetOutputPortOptimOpts(S, 30, SS_REUSABLE_AND_LOCAL);
31962
31963 /* outport number: 31 */
31964 if (!ssSetOutputPortVectorDimension(S, 31, 1))
31965 return;
31966 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31967 ssSetOutputPortDataType(S, 31, SS_SINGLE);
31968 }
31969
31970 ssSetOutputPortSampleTime(S, 31, 0.0);
31971 ssSetOutputPortOffsetTime(S, 31, 0.0);
31972 ssSetOutputPortOptimOpts(S, 31, SS_REUSABLE_AND_LOCAL);
31973
31974 /* outport number: 32 */
31975 if (!ssSetOutputPortVectorDimension(S, 32, 1))
31976 return;
31977 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31978 ssSetOutputPortDataType(S, 32, SS_INT8);
31979 }
31980
31981 ssSetOutputPortSampleTime(S, 32, 0.005);
31982 ssSetOutputPortOffsetTime(S, 32, 0.0);
31983 ssSetOutputPortOptimOpts(S, 32, SS_REUSABLE_AND_LOCAL);
31984
31985 /* Number of input ports */
31986 if (!ssSetNumInputPorts(S, 12))
31987 return;
31988
31989 /* inport number: 0 */
31990 {
31991 if (!ssSetInputPortVectorDimension(S, 0, 1))
31992 return;
31993 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
31994 ssSetInputPortDataType(S, 0, SS_INT8);
31995 }
31996
31997 ssSetInputPortDirectFeedThrough(S, 0, 1);
31998 ssSetInputPortSampleTime(S, 0, 0.0);
31999 ssSetInputPortOffsetTime(S, 0, 0.0);
32000 ssSetInputPortOverWritable(S, 0, 0);
32001 ssSetInputPortOptimOpts(S, 0, SS_NOT_REUSABLE_AND_GLOBAL);
32002 }
32003
32004 /* inport number: 1 */
32005 {
32006 if (!ssSetInputPortVectorDimension(S, 1, 1))
32007 return;
32008 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
32009 ssSetInputPortDataType(S, 1, SS_INT8);
32010 }
32011
32012 ssSetInputPortDirectFeedThrough(S, 1, 1);
32013 ssSetInputPortSampleTime(S, 1, 0.0);
32014 ssSetInputPortOffsetTime(S, 1, 0.0);
32015 ssSetInputPortOverWritable(S, 1, 0);
32016 ssSetInputPortOptimOpts(S, 1, SS_NOT_REUSABLE_AND_GLOBAL);
32017 }
32018
32019 /* inport number: 2 */
32020 {
32021 if (!ssSetInputPortVectorDimension(S, 2, 1))
32022 return;
32023 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
32024 ssSetInputPortDataType(S, 2, SS_SINGLE);
32025 }
32026
32027 ssSetInputPortDirectFeedThrough(S, 2, 1);
32028 ssSetInputPortSampleTime(S, 2, 0.0);
32029 ssSetInputPortOffsetTime(S, 2, 0.0);
32030 ssSetInputPortOverWritable(S, 2, 0);
32031 ssSetInputPortOptimOpts(S, 2, SS_NOT_REUSABLE_AND_GLOBAL);
32032 }
32033
32034 /* inport number: 3 */
32035 {
32036 if (!ssSetInputPortVectorDimension(S, 3, 1))
32037 return;
32038 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
32039 ssSetInputPortDataType(S, 3, SS_SINGLE);
32040 }
32041
32042 ssSetInputPortDirectFeedThrough(S, 3, 1);
32043 ssSetInputPortSampleTime(S, 3, 0.0);
32044 ssSetInputPortOffsetTime(S, 3, 0.0);
32045 ssSetInputPortOverWritable(S, 3, 0);
32046 ssSetInputPortOptimOpts(S, 3, SS_NOT_REUSABLE_AND_GLOBAL);
32047 }
32048
32049 /* inport number: 4 */
32050 {
32051 if (!ssSetInputPortVectorDimension(S, 4, 1))
32052 return;
32053 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
32054 ssSetInputPortDataType(S, 4, SS_SINGLE);
32055 }
32056
32057 ssSetInputPortDirectFeedThrough(S, 4, 1);
32058 ssSetInputPortSampleTime(S, 4, 0.0);
32059 ssSetInputPortOffsetTime(S, 4, 0.0);
32060 ssSetInputPortOverWritable(S, 4, 0);
32061 ssSetInputPortOptimOpts(S, 4, SS_NOT_REUSABLE_AND_GLOBAL);
32062 }
32063
32064 /* inport number: 5 */
32065 {
32066 if (!ssSetInputPortVectorDimension(S, 5, 1))
32067 return;
32068 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
32069 ssSetInputPortDataType(S, 5, SS_DOUBLE);
32070 }
32071
32072 ssSetInputPortDirectFeedThrough(S, 5, 1);
32073 ssSetInputPortSampleTime(S, 5, 0.0);
32074 ssSetInputPortOffsetTime(S, 5, 0.0);
32075 ssSetInputPortOverWritable(S, 5, 0);
32076 ssSetInputPortOptimOpts(S, 5, SS_NOT_REUSABLE_AND_GLOBAL);
32077 }
32078
32079 /* inport number: 6 */
32080 {
32081 if (!ssSetInputPortVectorDimension(S, 6, 1))
32082 return;
32083 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
32084 ssSetInputPortDataType(S, 6, SS_DOUBLE);
32085 }
32086
32087 ssSetInputPortDirectFeedThrough(S, 6, 1);
32088 ssSetInputPortSampleTime(S, 6, 0.0);
32089 ssSetInputPortOffsetTime(S, 6, 0.0);
32090 ssSetInputPortOverWritable(S, 6, 0);
32091 ssSetInputPortOptimOpts(S, 6, SS_NOT_REUSABLE_AND_GLOBAL);
32092 }
32093
32094 /* inport number: 7 */
32095 {
32096 if (!ssSetInputPortVectorDimension(S, 7, 1))
32097 return;
32098 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
32099 ssSetInputPortDataType(S, 7, SS_DOUBLE);
32100 }
32101
32102 ssSetInputPortDirectFeedThrough(S, 7, 1);
32103 ssSetInputPortSampleTime(S, 7, 0.0);
32104 ssSetInputPortOffsetTime(S, 7, 0.0);
32105 ssSetInputPortOverWritable(S, 7, 0);
32106 ssSetInputPortOptimOpts(S, 7, SS_NOT_REUSABLE_AND_GLOBAL);
32107 }
32108
32109 /* inport number: 8 */
32110 {
32111 if (!ssSetInputPortMatrixDimensions(S, 8, 3, 30))
32112 return;
32113 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
32114 ssSetInputPortDataType(S, 8, SS_DOUBLE);
32115 }
32116
32117 ssSetInputPortDirectFeedThrough(S, 8, 1);
32118 ssSetInputPortSampleTime(S, 8, 0.0);
32119 ssSetInputPortOffsetTime(S, 8, 0.0);
32120 ssSetInputPortOverWritable(S, 8, 0);
32121 ssSetInputPortOptimOpts(S, 8, SS_NOT_REUSABLE_AND_GLOBAL);
32122 }
32123
32124 /* inport number: 9 */
32125 {
32126 if (!ssSetInputPortVectorDimension(S, 9, 1))
32127 return;
32128 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
32129 ssSetInputPortDataType(S, 9, SS_DOUBLE);
32130 }
32131
32132 ssSetInputPortDirectFeedThrough(S, 9, 1);
32133 ssSetInputPortSampleTime(S, 9, 0.0);
32134 ssSetInputPortOffsetTime(S, 9, 0.0);
32135 ssSetInputPortOverWritable(S, 9, 0);
32136 ssSetInputPortOptimOpts(S, 9, SS_NOT_REUSABLE_AND_GLOBAL);
32137 }
32138
32139 /* inport number: 10 */
32140 {
32141 if (!ssSetInputPortVectorDimension(S, 10, 1))
32142 return;
32143 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
32144 ssSetInputPortDataType(S, 10, SS_DOUBLE);
32145 }
32146
32147 ssSetInputPortDirectFeedThrough(S, 10, 1);
32148 ssSetInputPortSampleTime(S, 10, 0.0);
32149 ssSetInputPortOffsetTime(S, 10, 0.0);
32150 ssSetInputPortOverWritable(S, 10, 0);
32151 ssSetInputPortOptimOpts(S, 10, SS_NOT_REUSABLE_AND_GLOBAL);
32152 }
32153
32154 /* inport number: 11 */
32155 {
32156 if (!ssSetInputPortVectorDimension(S, 11, 1))
32157 return;
32158 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
32159 ssSetInputPortDataType(S, 11, SS_DOUBLE);
32160 }
32161
32162 ssSetInputPortDirectFeedThrough(S, 11, 1);
32163 ssSetInputPortSampleTime(S, 11, 0.0);
32164 ssSetInputPortOffsetTime(S, 11, 0.0);
32165 ssSetInputPortOverWritable(S, 11, 0);
32166 ssSetInputPortOptimOpts(S, 11, SS_NOT_REUSABLE_AND_GLOBAL);
32167 }
32168
32169 ssSetRTWGeneratedSFcn(S, 1); /* Generated S-function */
32170
32171 /* DWork */
32172 if (!ssSetNumDWork(S, 641)) {
32173 return;
32174 }
32175
32176 /* '<S619>/Unit Delay1': DSTATE */
32177 ssSetDWorkName(S, 0, "DWORK0");
32178 ssSetDWorkWidth(S, 0, 1);
32179 ssSetDWorkUsedAsDState(S, 0, 1);
32180
32181 /* '<S423>/Output': DSTATE */
32182 ssSetDWorkName(S, 1, "DWORK1");
32183 ssSetDWorkWidth(S, 1, 1);
32184 ssFxpSetU32BitRegionCompliant(S, 1);
32185 if (ssGetSimMode(S) != SS_SIMMODE_SIZES_CALL_ONLY) {
32186
32187#if defined (MATLAB_MEX_FILE)
32188
32189 {
32190 DTypeId dataTypeIdReg =
32191 ssRegisterDataTypeFxpFSlopeFixExpBias(
32192 S,
32193 0,
32194 64,
32195 1.0,
32196 0,
32197 0.0,
32198 0 /* false means do NOT obey data type override setting for this subsystem */
32199 );
32200 ssSetDWorkDataType(S, 1, dataTypeIdReg );
32201 }
32202
32203#endif
32204
32205 }
32206
32207 ssSetDWorkUsedAsDState(S, 1, 1);
32208
32209 /* '<S465>/Rate Limiter': PrevY */
32210 ssSetDWorkName(S, 2, "DWORK2");
32211 ssSetDWorkWidth(S, 2, 3);
32212
32213 /* '<S465>/Rate Limiter': LastMajorTime */
32214 ssSetDWorkName(S, 3, "DWORK3");
32215 ssSetDWorkWidth(S, 3, 1);
32216
32217 /* '<S446>/Backlash': DWORK1 */
32218 ssSetDWorkName(S, 4, "DWORK4");
32219 ssSetDWorkWidth(S, 4, 1);
32220
32221 /* '<S954>/WGS84 Gravity S-Function': h */
32222 ssSetDWorkName(S, 5, "DWORK5");
32223 ssSetDWorkWidth(S, 5, 1);
32224
32225 /* '<S954>/WGS84 Gravity S-Function': phi */
32226 ssSetDWorkName(S, 6, "DWORK6");
32227 ssSetDWorkWidth(S, 6, 1);
32228
32229 /* '<S954>/WGS84 Gravity S-Function': lambda */
32230 ssSetDWorkName(S, 7, "DWORK7");
32231 ssSetDWorkWidth(S, 7, 1);
32232
32233 /* '<S954>/WGS84 Gravity S-Function': gamma_h */
32234 ssSetDWorkName(S, 8, "DWORK8");
32235 ssSetDWorkWidth(S, 8, 1);
32236
32237 /* '<S954>/WGS84 Gravity S-Function': gamma_phi */
32238 ssSetDWorkName(S, 9, "DWORK9");
32239 ssSetDWorkWidth(S, 9, 1);
32240
32241 /* '<S777>/Product2': DWORK4 */
32242 ssSetDWorkName(S, 10, "DWORK10");
32243 ssSetDWorkWidth(S, 10, 9);
32244
32245 /* '<S793>/Memory': PreviousInput */
32246 ssSetDWorkName(S, 11, "DWORK11");
32247 ssSetDWorkWidth(S, 11, 1);
32248
32249 /* '<S335>/Data Store Memory10': dsmMem */
32250 ssSetDWorkName(S, 12, "DWORK12");
32251 ssSetDWorkWidth(S, 12, 32);
32252
32253 /* '<S335>/Data Store Memory5': dsmMem */
32254 ssSetDWorkName(S, 13, "DWORK13");
32255 ssSetDWorkWidth(S, 13, 32);
32256
32257 /* '<S335>/Data Store Memory6': dsmMem */
32258 ssSetDWorkName(S, 14, "DWORK14");
32259 ssSetDWorkWidth(S, 14, 32);
32260
32261 /* '<S335>/Data Store Memory7': dsmMem */
32262 ssSetDWorkName(S, 15, "DWORK15");
32263 ssSetDWorkWidth(S, 15, 32);
32264
32265 /* '<S335>/Data Store Memory8': dsmMem */
32266 ssSetDWorkName(S, 16, "DWORK16");
32267 ssSetDWorkWidth(S, 16, 32);
32268
32269 /* '<S335>/Data Store Memory9': dsmMem */
32270 ssSetDWorkName(S, 17, "DWORK17");
32271 ssSetDWorkWidth(S, 17, 32);
32272
32273 /* '<S6>/Nz': PWORK */
32274 ssSetDWorkName(S, 18, "DWORK18");
32275 ssSetDWorkWidth(S, 18, 1);
32276 ssSetDWorkDataType(S, 18, SS_POINTER);
32277
32278 /* '<S6>/PhiL1': PWORK */
32279 ssSetDWorkName(S, 19, "DWORK19");
32280 ssSetDWorkWidth(S, 19, 2);
32281 ssSetDWorkDataType(S, 19, SS_POINTER);
32282
32283 /* '<S6>/PhiL2': PWORK */
32284 ssSetDWorkName(S, 20, "DWORK20");
32285 ssSetDWorkWidth(S, 20, 2);
32286 ssSetDWorkDataType(S, 20, SS_POINTER);
32287
32288 /* '<S6>/PhiL3': PWORK */
32289 ssSetDWorkName(S, 21, "DWORK21");
32290 ssSetDWorkWidth(S, 21, 2);
32291 ssSetDWorkDataType(S, 21, SS_POINTER);
32292
32293 /* '<S6>/PhiL4': PWORK */
32294 ssSetDWorkName(S, 22, "DWORK22");
32295 ssSetDWorkWidth(S, 22, 2);
32296 ssSetDWorkDataType(S, 22, SS_POINTER);
32297
32298 /* '<S6>/Scope': PWORK */
32299 ssSetDWorkName(S, 23, "DWORK23");
32300 ssSetDWorkWidth(S, 23, 1);
32301 ssSetDWorkDataType(S, 23, SS_POINTER);
32302
32303 /* '<S6>/Scope1': PWORK */
32304 ssSetDWorkName(S, 24, "DWORK24");
32305 ssSetDWorkWidth(S, 24, 1);
32306 ssSetDWorkDataType(S, 24, SS_POINTER);
32307
32308 /* '<S6>/Scope10': PWORK */
32309 ssSetDWorkName(S, 25, "DWORK25");
32310 ssSetDWorkWidth(S, 25, 1);
32311 ssSetDWorkDataType(S, 25, SS_POINTER);
32312
32313 /* '<S6>/Scope11': PWORK */
32314 ssSetDWorkName(S, 26, "DWORK26");
32315 ssSetDWorkWidth(S, 26, 1);
32316 ssSetDWorkDataType(S, 26, SS_POINTER);
32317
32318 /* '<S6>/Scope12': PWORK */
32319 ssSetDWorkName(S, 27, "DWORK27");
32320 ssSetDWorkWidth(S, 27, 1);
32321 ssSetDWorkDataType(S, 27, SS_POINTER);
32322
32323 /* '<S6>/Scope14': PWORK */
32324 ssSetDWorkName(S, 28, "DWORK28");
32325 ssSetDWorkWidth(S, 28, 1);
32326 ssSetDWorkDataType(S, 28, SS_POINTER);
32327
32328 /* '<S6>/Scope15': PWORK */
32329 ssSetDWorkName(S, 29, "DWORK29");
32330 ssSetDWorkWidth(S, 29, 1);
32331 ssSetDWorkDataType(S, 29, SS_POINTER);
32332
32333 /* '<S6>/Scope16': PWORK */
32334 ssSetDWorkName(S, 30, "DWORK30");
32335 ssSetDWorkWidth(S, 30, 1);
32336 ssSetDWorkDataType(S, 30, SS_POINTER);
32337
32338 /* '<S6>/Scope17': PWORK */
32339 ssSetDWorkName(S, 31, "DWORK31");
32340 ssSetDWorkWidth(S, 31, 1);
32341 ssSetDWorkDataType(S, 31, SS_POINTER);
32342
32343 /* '<S6>/Scope18': PWORK */
32344 ssSetDWorkName(S, 32, "DWORK32");
32345 ssSetDWorkWidth(S, 32, 1);
32346 ssSetDWorkDataType(S, 32, SS_POINTER);
32347
32348 /* '<S6>/Scope19': PWORK */
32349 ssSetDWorkName(S, 33, "DWORK33");
32350 ssSetDWorkWidth(S, 33, 1);
32351 ssSetDWorkDataType(S, 33, SS_POINTER);
32352
32353 /* '<S6>/Scope2': PWORK */
32354 ssSetDWorkName(S, 34, "DWORK34");
32355 ssSetDWorkWidth(S, 34, 1);
32356 ssSetDWorkDataType(S, 34, SS_POINTER);
32357
32358 /* '<S6>/Scope20': PWORK */
32359 ssSetDWorkName(S, 35, "DWORK35");
32360 ssSetDWorkWidth(S, 35, 1);
32361 ssSetDWorkDataType(S, 35, SS_POINTER);
32362
32363 /* '<S6>/Scope21': PWORK */
32364 ssSetDWorkName(S, 36, "DWORK36");
32365 ssSetDWorkWidth(S, 36, 1);
32366 ssSetDWorkDataType(S, 36, SS_POINTER);
32367
32368 /* '<S6>/Scope22': PWORK */
32369 ssSetDWorkName(S, 37, "DWORK37");
32370 ssSetDWorkWidth(S, 37, 1);
32371 ssSetDWorkDataType(S, 37, SS_POINTER);
32372
32373 /* '<S6>/Scope23': PWORK */
32374 ssSetDWorkName(S, 38, "DWORK38");
32375 ssSetDWorkWidth(S, 38, 1);
32376 ssSetDWorkDataType(S, 38, SS_POINTER);
32377
32378 /* '<S6>/Scope24': PWORK */
32379 ssSetDWorkName(S, 39, "DWORK39");
32380 ssSetDWorkWidth(S, 39, 1);
32381 ssSetDWorkDataType(S, 39, SS_POINTER);
32382
32383 /* '<S6>/Scope25': PWORK */
32384 ssSetDWorkName(S, 40, "DWORK40");
32385 ssSetDWorkWidth(S, 40, 1);
32386 ssSetDWorkDataType(S, 40, SS_POINTER);
32387
32388 /* '<S6>/Scope26': PWORK */
32389 ssSetDWorkName(S, 41, "DWORK41");
32390 ssSetDWorkWidth(S, 41, 1);
32391 ssSetDWorkDataType(S, 41, SS_POINTER);
32392
32393 /* '<S6>/Scope27': PWORK */
32394 ssSetDWorkName(S, 42, "DWORK42");
32395 ssSetDWorkWidth(S, 42, 1);
32396 ssSetDWorkDataType(S, 42, SS_POINTER);
32397
32398 /* '<S6>/Scope28': PWORK */
32399 ssSetDWorkName(S, 43, "DWORK43");
32400 ssSetDWorkWidth(S, 43, 1);
32401 ssSetDWorkDataType(S, 43, SS_POINTER);
32402
32403 /* '<S6>/Scope29': PWORK */
32404 ssSetDWorkName(S, 44, "DWORK44");
32405 ssSetDWorkWidth(S, 44, 1);
32406 ssSetDWorkDataType(S, 44, SS_POINTER);
32407
32408 /* '<S6>/Scope3': PWORK */
32409 ssSetDWorkName(S, 45, "DWORK45");
32410 ssSetDWorkWidth(S, 45, 1);
32411 ssSetDWorkDataType(S, 45, SS_POINTER);
32412
32413 /* '<S6>/Scope30': PWORK */
32414 ssSetDWorkName(S, 46, "DWORK46");
32415 ssSetDWorkWidth(S, 46, 1);
32416 ssSetDWorkDataType(S, 46, SS_POINTER);
32417
32418 /* '<S6>/Scope31': PWORK */
32419 ssSetDWorkName(S, 47, "DWORK47");
32420 ssSetDWorkWidth(S, 47, 1);
32421 ssSetDWorkDataType(S, 47, SS_POINTER);
32422
32423 /* '<S6>/Scope32': PWORK */
32424 ssSetDWorkName(S, 48, "DWORK48");
32425 ssSetDWorkWidth(S, 48, 1);
32426 ssSetDWorkDataType(S, 48, SS_POINTER);
32427
32428 /* '<S6>/Scope33': PWORK */
32429 ssSetDWorkName(S, 49, "DWORK49");
32430 ssSetDWorkWidth(S, 49, 1);
32431 ssSetDWorkDataType(S, 49, SS_POINTER);
32432
32433 /* '<S6>/Scope34': PWORK */
32434 ssSetDWorkName(S, 50, "DWORK50");
32435 ssSetDWorkWidth(S, 50, 1);
32436 ssSetDWorkDataType(S, 50, SS_POINTER);
32437
32438 /* '<S6>/Scope35': PWORK */
32439 ssSetDWorkName(S, 51, "DWORK51");
32440 ssSetDWorkWidth(S, 51, 1);
32441 ssSetDWorkDataType(S, 51, SS_POINTER);
32442
32443 /* '<S6>/Scope36': PWORK */
32444 ssSetDWorkName(S, 52, "DWORK52");
32445 ssSetDWorkWidth(S, 52, 1);
32446 ssSetDWorkDataType(S, 52, SS_POINTER);
32447
32448 /* '<S6>/Scope37': PWORK */
32449 ssSetDWorkName(S, 53, "DWORK53");
32450 ssSetDWorkWidth(S, 53, 1);
32451 ssSetDWorkDataType(S, 53, SS_POINTER);
32452
32453 /* '<S6>/Scope38': PWORK */
32454 ssSetDWorkName(S, 54, "DWORK54");
32455 ssSetDWorkWidth(S, 54, 1);
32456 ssSetDWorkDataType(S, 54, SS_POINTER);
32457
32458 /* '<S6>/Scope39': PWORK */
32459 ssSetDWorkName(S, 55, "DWORK55");
32460 ssSetDWorkWidth(S, 55, 1);
32461 ssSetDWorkDataType(S, 55, SS_POINTER);
32462
32463 /* '<S6>/Scope4': PWORK */
32464 ssSetDWorkName(S, 56, "DWORK56");
32465 ssSetDWorkWidth(S, 56, 1);
32466 ssSetDWorkDataType(S, 56, SS_POINTER);
32467
32468 /* '<S6>/Scope40': PWORK */
32469 ssSetDWorkName(S, 57, "DWORK57");
32470 ssSetDWorkWidth(S, 57, 1);
32471 ssSetDWorkDataType(S, 57, SS_POINTER);
32472
32473 /* '<S6>/Scope41': PWORK */
32474 ssSetDWorkName(S, 58, "DWORK58");
32475 ssSetDWorkWidth(S, 58, 1);
32476 ssSetDWorkDataType(S, 58, SS_POINTER);
32477
32478 /* '<S6>/Scope42': PWORK */
32479 ssSetDWorkName(S, 59, "DWORK59");
32480 ssSetDWorkWidth(S, 59, 1);
32481 ssSetDWorkDataType(S, 59, SS_POINTER);
32482
32483 /* '<S6>/Scope43': PWORK */
32484 ssSetDWorkName(S, 60, "DWORK60");
32485 ssSetDWorkWidth(S, 60, 1);
32486 ssSetDWorkDataType(S, 60, SS_POINTER);
32487
32488 /* '<S6>/Scope44': PWORK */
32489 ssSetDWorkName(S, 61, "DWORK61");
32490 ssSetDWorkWidth(S, 61, 1);
32491 ssSetDWorkDataType(S, 61, SS_POINTER);
32492
32493 /* '<S6>/Scope45': PWORK */
32494 ssSetDWorkName(S, 62, "DWORK62");
32495 ssSetDWorkWidth(S, 62, 1);
32496 ssSetDWorkDataType(S, 62, SS_POINTER);
32497
32498 /* '<S6>/Scope46': PWORK */
32499 ssSetDWorkName(S, 63, "DWORK63");
32500 ssSetDWorkWidth(S, 63, 1);
32501 ssSetDWorkDataType(S, 63, SS_POINTER);
32502
32503 /* '<S6>/Scope47': PWORK */
32504 ssSetDWorkName(S, 64, "DWORK64");
32505 ssSetDWorkWidth(S, 64, 1);
32506 ssSetDWorkDataType(S, 64, SS_POINTER);
32507
32508 /* '<S6>/Scope48': PWORK */
32509 ssSetDWorkName(S, 65, "DWORK65");
32510 ssSetDWorkWidth(S, 65, 1);
32511 ssSetDWorkDataType(S, 65, SS_POINTER);
32512
32513 /* '<S6>/Scope49': PWORK */
32514 ssSetDWorkName(S, 66, "DWORK66");
32515 ssSetDWorkWidth(S, 66, 1);
32516 ssSetDWorkDataType(S, 66, SS_POINTER);
32517
32518 /* '<S6>/Scope5': PWORK */
32519 ssSetDWorkName(S, 67, "DWORK67");
32520 ssSetDWorkWidth(S, 67, 1);
32521 ssSetDWorkDataType(S, 67, SS_POINTER);
32522
32523 /* '<S6>/Scope50': PWORK */
32524 ssSetDWorkName(S, 68, "DWORK68");
32525 ssSetDWorkWidth(S, 68, 1);
32526 ssSetDWorkDataType(S, 68, SS_POINTER);
32527
32528 /* '<S6>/Scope51': PWORK */
32529 ssSetDWorkName(S, 69, "DWORK69");
32530 ssSetDWorkWidth(S, 69, 1);
32531 ssSetDWorkDataType(S, 69, SS_POINTER);
32532
32533 /* '<S6>/Scope52': PWORK */
32534 ssSetDWorkName(S, 70, "DWORK70");
32535 ssSetDWorkWidth(S, 70, 1);
32536 ssSetDWorkDataType(S, 70, SS_POINTER);
32537
32538 /* '<S6>/Scope53': PWORK */
32539 ssSetDWorkName(S, 71, "DWORK71");
32540 ssSetDWorkWidth(S, 71, 1);
32541 ssSetDWorkDataType(S, 71, SS_POINTER);
32542
32543 /* '<S6>/Scope54': PWORK */
32544 ssSetDWorkName(S, 72, "DWORK72");
32545 ssSetDWorkWidth(S, 72, 1);
32546 ssSetDWorkDataType(S, 72, SS_POINTER);
32547
32548 /* '<S6>/Scope55': PWORK */
32549 ssSetDWorkName(S, 73, "DWORK73");
32550 ssSetDWorkWidth(S, 73, 1);
32551 ssSetDWorkDataType(S, 73, SS_POINTER);
32552
32553 /* '<S6>/Scope56': PWORK */
32554 ssSetDWorkName(S, 74, "DWORK74");
32555 ssSetDWorkWidth(S, 74, 1);
32556 ssSetDWorkDataType(S, 74, SS_POINTER);
32557
32558 /* '<S6>/Scope57': PWORK */
32559 ssSetDWorkName(S, 75, "DWORK75");
32560 ssSetDWorkWidth(S, 75, 1);
32561 ssSetDWorkDataType(S, 75, SS_POINTER);
32562
32563 /* '<S6>/Scope58': PWORK */
32564 ssSetDWorkName(S, 76, "DWORK76");
32565 ssSetDWorkWidth(S, 76, 1);
32566 ssSetDWorkDataType(S, 76, SS_POINTER);
32567
32568 /* '<S6>/Scope59': PWORK */
32569 ssSetDWorkName(S, 77, "DWORK77");
32570 ssSetDWorkWidth(S, 77, 1);
32571 ssSetDWorkDataType(S, 77, SS_POINTER);
32572
32573 /* '<S6>/Scope6': PWORK */
32574 ssSetDWorkName(S, 78, "DWORK78");
32575 ssSetDWorkWidth(S, 78, 1);
32576 ssSetDWorkDataType(S, 78, SS_POINTER);
32577
32578 /* '<S6>/Scope60': PWORK */
32579 ssSetDWorkName(S, 79, "DWORK79");
32580 ssSetDWorkWidth(S, 79, 1);
32581 ssSetDWorkDataType(S, 79, SS_POINTER);
32582
32583 /* '<S6>/Scope61': PWORK */
32584 ssSetDWorkName(S, 80, "DWORK80");
32585 ssSetDWorkWidth(S, 80, 2);
32586 ssSetDWorkDataType(S, 80, SS_POINTER);
32587
32588 /* '<S6>/Scope62': PWORK */
32589 ssSetDWorkName(S, 81, "DWORK81");
32590 ssSetDWorkWidth(S, 81, 1);
32591 ssSetDWorkDataType(S, 81, SS_POINTER);
32592
32593 /* '<S6>/Scope63': PWORK */
32594 ssSetDWorkName(S, 82, "DWORK82");
32595 ssSetDWorkWidth(S, 82, 1);
32596 ssSetDWorkDataType(S, 82, SS_POINTER);
32597
32598 /* '<S6>/Scope64': PWORK */
32599 ssSetDWorkName(S, 83, "DWORK83");
32600 ssSetDWorkWidth(S, 83, 1);
32601 ssSetDWorkDataType(S, 83, SS_POINTER);
32602
32603 /* '<S6>/Scope65': PWORK */
32604 ssSetDWorkName(S, 84, "DWORK84");
32605 ssSetDWorkWidth(S, 84, 1);
32606 ssSetDWorkDataType(S, 84, SS_POINTER);
32607
32608 /* '<S6>/Scope66': PWORK */
32609 ssSetDWorkName(S, 85, "DWORK85");
32610 ssSetDWorkWidth(S, 85, 1);
32611 ssSetDWorkDataType(S, 85, SS_POINTER);
32612
32613 /* '<S6>/Scope69': PWORK */
32614 ssSetDWorkName(S, 86, "DWORK86");
32615 ssSetDWorkWidth(S, 86, 1);
32616 ssSetDWorkDataType(S, 86, SS_POINTER);
32617
32618 /* '<S6>/Scope7': PWORK */
32619 ssSetDWorkName(S, 87, "DWORK87");
32620 ssSetDWorkWidth(S, 87, 1);
32621 ssSetDWorkDataType(S, 87, SS_POINTER);
32622
32623 /* '<S6>/Scope70': PWORK */
32624 ssSetDWorkName(S, 88, "DWORK88");
32625 ssSetDWorkWidth(S, 88, 1);
32626 ssSetDWorkDataType(S, 88, SS_POINTER);
32627
32628 /* '<S6>/Scope71': PWORK */
32629 ssSetDWorkName(S, 89, "DWORK89");
32630 ssSetDWorkWidth(S, 89, 3);
32631 ssSetDWorkDataType(S, 89, SS_POINTER);
32632
32633 /* '<S6>/Scope72': PWORK */
32634 ssSetDWorkName(S, 90, "DWORK90");
32635 ssSetDWorkWidth(S, 90, 1);
32636 ssSetDWorkDataType(S, 90, SS_POINTER);
32637
32638 /* '<S6>/Scope73': PWORK */
32639 ssSetDWorkName(S, 91, "DWORK91");
32640 ssSetDWorkWidth(S, 91, 1);
32641 ssSetDWorkDataType(S, 91, SS_POINTER);
32642
32643 /* '<S6>/Scope75': PWORK */
32644 ssSetDWorkName(S, 92, "DWORK92");
32645 ssSetDWorkWidth(S, 92, 1);
32646 ssSetDWorkDataType(S, 92, SS_POINTER);
32647
32648 /* '<S6>/Scope8': PWORK */
32649 ssSetDWorkName(S, 93, "DWORK93");
32650 ssSetDWorkWidth(S, 93, 1);
32651 ssSetDWorkDataType(S, 93, SS_POINTER);
32652
32653 /* '<S6>/Scope9': PWORK */
32654 ssSetDWorkName(S, 94, "DWORK94");
32655 ssSetDWorkWidth(S, 94, 1);
32656 ssSetDWorkDataType(S, 94, SS_POINTER);
32657
32658 /* '<S6>/Vc': PWORK */
32659 ssSetDWorkName(S, 95, "DWORK95");
32660 ssSetDWorkWidth(S, 95, 1);
32661 ssSetDWorkDataType(S, 95, SS_POINTER);
32662
32663 /* '<S6>/alfa': PWORK */
32664 ssSetDWorkName(S, 96, "DWORK96");
32665 ssSetDWorkWidth(S, 96, 1);
32666 ssSetDWorkDataType(S, 96, SS_POINTER);
32667
32668 /* '<S6>/alfa1': PWORK */
32669 ssSetDWorkName(S, 97, "DWORK97");
32670 ssSetDWorkWidth(S, 97, 1);
32671 ssSetDWorkDataType(S, 97, SS_POINTER);
32672
32673 /* '<S6>/alfa2': PWORK */
32674 ssSetDWorkName(S, 98, "DWORK98");
32675 ssSetDWorkWidth(S, 98, 1);
32676 ssSetDWorkDataType(S, 98, SS_POINTER);
32677
32678 /* '<S6>/alfa4': PWORK */
32679 ssSetDWorkName(S, 99, "DWORK99");
32680 ssSetDWorkWidth(S, 99, 1);
32681 ssSetDWorkDataType(S, 99, SS_POINTER);
32682
32683 /* '<S6>/alfa5': PWORK */
32684 ssSetDWorkName(S, 100, "DWORK100");
32685 ssSetDWorkWidth(S, 100, 1);
32686 ssSetDWorkDataType(S, 100, SS_POINTER);
32687
32688 /* '<S6>/beta': PWORK */
32689 ssSetDWorkName(S, 101, "DWORK101");
32690 ssSetDWorkWidth(S, 101, 1);
32691 ssSetDWorkDataType(S, 101, SS_POINTER);
32692
32693 /* '<S6>/k': PWORK */
32694 ssSetDWorkName(S, 102, "DWORK102");
32695 ssSetDWorkWidth(S, 102, 1);
32696 ssSetDWorkDataType(S, 102, SS_POINTER);
32697
32698 /* '<S6>/k1': PWORK */
32699 ssSetDWorkName(S, 103, "DWORK103");
32700 ssSetDWorkWidth(S, 103, 1);
32701 ssSetDWorkDataType(S, 103, SS_POINTER);
32702
32703 /* '<S6>/psd': PWORK */
32704 ssSetDWorkName(S, 104, "DWORK104");
32705 ssSetDWorkWidth(S, 104, 1);
32706 ssSetDWorkDataType(S, 104, SS_POINTER);
32707
32708 /* '<S6>/thetaL1': PWORK */
32709 ssSetDWorkName(S, 105, "DWORK105");
32710 ssSetDWorkWidth(S, 105, 1);
32711 ssSetDWorkDataType(S, 105, SS_POINTER);
32712
32713 /* '<S6>/thetaL2': PWORK */
32714 ssSetDWorkName(S, 106, "DWORK106");
32715 ssSetDWorkWidth(S, 106, 1);
32716 ssSetDWorkDataType(S, 106, SS_POINTER);
32717
32718 /* '<S1>/Scope': PWORK */
32719 ssSetDWorkName(S, 107, "DWORK107");
32720 ssSetDWorkWidth(S, 107, 1);
32721 ssSetDWorkDataType(S, 107, SS_POINTER);
32722
32723 /* '<S1>/Scope1': PWORK */
32724 ssSetDWorkName(S, 108, "DWORK108");
32725 ssSetDWorkWidth(S, 108, 1);
32726 ssSetDWorkDataType(S, 108, SS_POINTER);
32727
32728 /* '<S1>/Scope2': PWORK */
32729 ssSetDWorkName(S, 109, "DWORK109");
32730 ssSetDWorkWidth(S, 109, 1);
32731 ssSetDWorkDataType(S, 109, SS_POINTER);
32732
32733 /* '<S1>/To Workspace': PWORK */
32734 ssSetDWorkName(S, 110, "DWORK110");
32735 ssSetDWorkWidth(S, 110, 1);
32736 ssSetDWorkDataType(S, 110, SS_POINTER);
32737
32738 /* '<S31>/Scope': PWORK */
32739 ssSetDWorkName(S, 111, "DWORK111");
32740 ssSetDWorkWidth(S, 111, 1);
32741 ssSetDWorkDataType(S, 111, SS_POINTER);
32742
32743 /* '<S9>/Scope': PWORK */
32744 ssSetDWorkName(S, 112, "DWORK112");
32745 ssSetDWorkWidth(S, 112, 1);
32746 ssSetDWorkDataType(S, 112, SS_POINTER);
32747
32748 /* '<S9>/Scope1': PWORK */
32749 ssSetDWorkName(S, 113, "DWORK113");
32750 ssSetDWorkWidth(S, 113, 1);
32751 ssSetDWorkDataType(S, 113, SS_POINTER);
32752
32753 /* '<S9>/Scope2': PWORK */
32754 ssSetDWorkName(S, 114, "DWORK114");
32755 ssSetDWorkWidth(S, 114, 1);
32756 ssSetDWorkDataType(S, 114, SS_POINTER);
32757
32758 /* '<S9>/Scope3': PWORK */
32759 ssSetDWorkName(S, 115, "DWORK115");
32760 ssSetDWorkWidth(S, 115, 1);
32761 ssSetDWorkDataType(S, 115, SS_POINTER);
32762
32763 /* '<S9>/Scope4': PWORK */
32764 ssSetDWorkName(S, 116, "DWORK116");
32765 ssSetDWorkWidth(S, 116, 1);
32766 ssSetDWorkDataType(S, 116, SS_POINTER);
32767
32768 /* '<S9>/Scope5': PWORK */
32769 ssSetDWorkName(S, 117, "DWORK117");
32770 ssSetDWorkWidth(S, 117, 1);
32771 ssSetDWorkDataType(S, 117, SS_POINTER);
32772
32773 /* '<S9>/Scope7': PWORK */
32774 ssSetDWorkName(S, 118, "DWORK118");
32775 ssSetDWorkWidth(S, 118, 1);
32776 ssSetDWorkDataType(S, 118, SS_POINTER);
32777
32778 /* '<S9>/To Workspace': PWORK */
32779 ssSetDWorkName(S, 119, "DWORK119");
32780 ssSetDWorkWidth(S, 119, 1);
32781 ssSetDWorkDataType(S, 119, SS_POINTER);
32782
32783 /* '<S9>/To Workspace1': PWORK */
32784 ssSetDWorkName(S, 120, "DWORK120");
32785 ssSetDWorkWidth(S, 120, 1);
32786 ssSetDWorkDataType(S, 120, SS_POINTER);
32787
32788 /* '<S15>/Scope26': PWORK */
32789 ssSetDWorkName(S, 121, "DWORK121");
32790 ssSetDWorkWidth(S, 121, 3);
32791 ssSetDWorkDataType(S, 121, SS_POINTER);
32792
32793 /* '<S448>/Scope': PWORK */
32794 ssSetDWorkName(S, 122, "DWORK122");
32795 ssSetDWorkWidth(S, 122, 1);
32796 ssSetDWorkDataType(S, 122, SS_POINTER);
32797
32798 /* '<S449>/Scope': PWORK */
32799 ssSetDWorkName(S, 123, "DWORK123");
32800 ssSetDWorkWidth(S, 123, 1);
32801 ssSetDWorkDataType(S, 123, SS_POINTER);
32802
32803 /* '<S480>/Scope2': PWORK */
32804 ssSetDWorkName(S, 124, "DWORK124");
32805 ssSetDWorkWidth(S, 124, 1);
32806 ssSetDWorkDataType(S, 124, SS_POINTER);
32807
32808 /* '<S466>/Scope9': PWORK */
32809 ssSetDWorkName(S, 125, "DWORK125");
32810 ssSetDWorkWidth(S, 125, 1);
32811 ssSetDWorkDataType(S, 125, SS_POINTER);
32812
32813 /* '<S617>/Scope1': PWORK */
32814 ssSetDWorkName(S, 126, "DWORK126");
32815 ssSetDWorkWidth(S, 126, 1);
32816 ssSetDWorkDataType(S, 126, SS_POINTER);
32817
32818 /* '<S617>/Scope10': PWORK */
32819 ssSetDWorkName(S, 127, "DWORK127");
32820 ssSetDWorkWidth(S, 127, 1);
32821 ssSetDWorkDataType(S, 127, SS_POINTER);
32822
32823 /* '<S617>/Scope11': PWORK */
32824 ssSetDWorkName(S, 128, "DWORK128");
32825 ssSetDWorkWidth(S, 128, 1);
32826 ssSetDWorkDataType(S, 128, SS_POINTER);
32827
32828 /* '<S617>/Scope12': PWORK */
32829 ssSetDWorkName(S, 129, "DWORK129");
32830 ssSetDWorkWidth(S, 129, 1);
32831 ssSetDWorkDataType(S, 129, SS_POINTER);
32832
32833 /* '<S617>/Scope13': PWORK */
32834 ssSetDWorkName(S, 130, "DWORK130");
32835 ssSetDWorkWidth(S, 130, 1);
32836 ssSetDWorkDataType(S, 130, SS_POINTER);
32837
32838 /* '<S617>/Scope2': PWORK */
32839 ssSetDWorkName(S, 131, "DWORK131");
32840 ssSetDWorkWidth(S, 131, 2);
32841 ssSetDWorkDataType(S, 131, SS_POINTER);
32842
32843 /* '<S617>/Scope3': PWORK */
32844 ssSetDWorkName(S, 132, "DWORK132");
32845 ssSetDWorkWidth(S, 132, 2);
32846 ssSetDWorkDataType(S, 132, SS_POINTER);
32847
32848 /* '<S617>/Scope4': PWORK */
32849 ssSetDWorkName(S, 133, "DWORK133");
32850 ssSetDWorkWidth(S, 133, 1);
32851 ssSetDWorkDataType(S, 133, SS_POINTER);
32852
32853 /* '<S617>/Scope5': PWORK */
32854 ssSetDWorkName(S, 134, "DWORK134");
32855 ssSetDWorkWidth(S, 134, 1);
32856 ssSetDWorkDataType(S, 134, SS_POINTER);
32857
32858 /* '<S617>/Scope6': PWORK */
32859 ssSetDWorkName(S, 135, "DWORK135");
32860 ssSetDWorkWidth(S, 135, 1);
32861 ssSetDWorkDataType(S, 135, SS_POINTER);
32862
32863 /* '<S617>/Scope7': PWORK */
32864 ssSetDWorkName(S, 136, "DWORK136");
32865 ssSetDWorkWidth(S, 136, 1);
32866 ssSetDWorkDataType(S, 136, SS_POINTER);
32867
32868 /* '<S617>/Scope8': PWORK */
32869 ssSetDWorkName(S, 137, "DWORK137");
32870 ssSetDWorkWidth(S, 137, 1);
32871 ssSetDWorkDataType(S, 137, SS_POINTER);
32872
32873 /* '<S617>/Scope9': PWORK */
32874 ssSetDWorkName(S, 138, "DWORK138");
32875 ssSetDWorkWidth(S, 138, 1);
32876 ssSetDWorkDataType(S, 138, SS_POINTER);
32877
32878 /* '<S619>/Scope8': PWORK */
32879 ssSetDWorkName(S, 139, "DWORK139");
32880 ssSetDWorkWidth(S, 139, 3);
32881 ssSetDWorkDataType(S, 139, SS_POINTER);
32882
32883 /* '<S682>/Scope2': PWORK */
32884 ssSetDWorkName(S, 140, "DWORK140");
32885 ssSetDWorkWidth(S, 140, 1);
32886 ssSetDWorkDataType(S, 140, SS_POINTER);
32887
32888 /* '<S682>/Scope1': PWORK */
32889 ssSetDWorkName(S, 141, "DWORK141");
32890 ssSetDWorkWidth(S, 141, 1);
32891 ssSetDWorkDataType(S, 141, SS_POINTER);
32892
32893 /* '<S683>/Scope8': PWORK */
32894 ssSetDWorkName(S, 142, "DWORK142");
32895 ssSetDWorkWidth(S, 142, 3);
32896 ssSetDWorkDataType(S, 142, SS_POINTER);
32897
32898 /* '<S335>/Scope1': PWORK */
32899 ssSetDWorkName(S, 143, "DWORK143");
32900 ssSetDWorkWidth(S, 143, 1);
32901 ssSetDWorkDataType(S, 143, SS_POINTER);
32902
32903 /* '<S335>/To Workspace': PWORK */
32904 ssSetDWorkName(S, 144, "DWORK144");
32905 ssSetDWorkWidth(S, 144, 1);
32906 ssSetDWorkDataType(S, 144, SS_POINTER);
32907
32908 /* '<S315>/Scope': PWORK */
32909 ssSetDWorkName(S, 145, "DWORK145");
32910 ssSetDWorkWidth(S, 145, 1);
32911 ssSetDWorkDataType(S, 145, SS_POINTER);
32912
32913 /* '<S175>/Unit Delay1': DSTATE */
32914 ssSetDWorkName(S, 146, "DWORK146");
32915 ssSetDWorkWidth(S, 146, 1);
32916 ssSetDWorkDataType(S, 146, SS_SINGLE);
32917 ssSetDWorkUsedAsDState(S, 146, 1);
32918
32919 /* '<S174>/Generated S-Function': DWORK0 */
32920 ssSetDWorkName(S, 147, "DWORK147");
32921 ssSetDWorkWidth(S, 147, 1);
32922 ssSetDWorkDataType(S, 147, SS_SINGLE);
32923 ssSetDWorkUsedAsDState(S, 147, 1);
32924
32925 /* '<S174>/Generated S-Function': DWORK1 */
32926 ssSetDWorkName(S, 148, "DWORK148");
32927 ssSetDWorkWidth(S, 148, 1);
32928 ssSetDWorkDataType(S, 148, SS_SINGLE);
32929 ssSetDWorkUsedAsDState(S, 148, 1);
32930
32931 /* '<S185>/Generated S-Function': DWORK0 */
32932 ssSetDWorkName(S, 149, "DWORK149");
32933 ssSetDWorkWidth(S, 149, 1);
32934 ssSetDWorkDataType(S, 149, SS_SINGLE);
32935 ssSetDWorkUsedAsDState(S, 149, 1);
32936
32937 /* '<S185>/Generated S-Function': DWORK1 */
32938 ssSetDWorkName(S, 150, "DWORK150");
32939 ssSetDWorkWidth(S, 150, 1);
32940 ssSetDWorkDataType(S, 150, SS_SINGLE);
32941 ssSetDWorkUsedAsDState(S, 150, 1);
32942
32943 /* '<S184>/Generated S-Function': DWORK0 */
32944 ssSetDWorkName(S, 151, "DWORK151");
32945 ssSetDWorkWidth(S, 151, 1);
32946 ssSetDWorkDataType(S, 151, SS_SINGLE);
32947 ssSetDWorkUsedAsDState(S, 151, 1);
32948
32949 /* '<S184>/Generated S-Function': DWORK1 */
32950 ssSetDWorkName(S, 152, "DWORK152");
32951 ssSetDWorkWidth(S, 152, 1);
32952 ssSetDWorkDataType(S, 152, SS_SINGLE);
32953 ssSetDWorkUsedAsDState(S, 152, 1);
32954
32955 /* '<S183>/Generated S-Function': DWORK0 */
32956 ssSetDWorkName(S, 153, "DWORK153");
32957 ssSetDWorkWidth(S, 153, 1);
32958 ssSetDWorkDataType(S, 153, SS_SINGLE);
32959 ssSetDWorkUsedAsDState(S, 153, 1);
32960
32961 /* '<S183>/Generated S-Function': DWORK1 */
32962 ssSetDWorkName(S, 154, "DWORK154");
32963 ssSetDWorkWidth(S, 154, 1);
32964 ssSetDWorkDataType(S, 154, SS_SINGLE);
32965 ssSetDWorkUsedAsDState(S, 154, 1);
32966
32967 /* '<S178>/Generated S-Function': DWORK0 */
32968 ssSetDWorkName(S, 155, "DWORK155");
32969 ssSetDWorkWidth(S, 155, 1);
32970 ssSetDWorkDataType(S, 155, SS_SINGLE);
32971 ssSetDWorkUsedAsDState(S, 155, 1);
32972
32973 /* '<S178>/Generated S-Function': DWORK1 */
32974 ssSetDWorkName(S, 156, "DWORK156");
32975 ssSetDWorkWidth(S, 156, 1);
32976 ssSetDWorkDataType(S, 156, SS_SINGLE);
32977 ssSetDWorkUsedAsDState(S, 156, 1);
32978
32979 /* '<S177>/Generated S-Function': DWORK0 */
32980 ssSetDWorkName(S, 157, "DWORK157");
32981 ssSetDWorkWidth(S, 157, 1);
32982 ssSetDWorkDataType(S, 157, SS_SINGLE);
32983 ssSetDWorkUsedAsDState(S, 157, 1);
32984
32985 /* '<S177>/Generated S-Function': DWORK1 */
32986 ssSetDWorkName(S, 158, "DWORK158");
32987 ssSetDWorkWidth(S, 158, 1);
32988 ssSetDWorkDataType(S, 158, SS_SINGLE);
32989 ssSetDWorkUsedAsDState(S, 158, 1);
32990
32991 /* '<S176>/Generated S-Function': DWORK0 */
32992 ssSetDWorkName(S, 159, "DWORK159");
32993 ssSetDWorkWidth(S, 159, 1);
32994 ssSetDWorkDataType(S, 159, SS_SINGLE);
32995 ssSetDWorkUsedAsDState(S, 159, 1);
32996
32997 /* '<S176>/Generated S-Function': DWORK1 */
32998 ssSetDWorkName(S, 160, "DWORK160");
32999 ssSetDWorkWidth(S, 160, 1);
33000 ssSetDWorkDataType(S, 160, SS_SINGLE);
33001 ssSetDWorkUsedAsDState(S, 160, 1);
33002
33003 /* '<S115>/Unit Delay1': DSTATE */
33004 ssSetDWorkName(S, 161, "DWORK161");
33005 ssSetDWorkWidth(S, 161, 1);
33006 ssSetDWorkDataType(S, 161, SS_SINGLE);
33007 ssSetDWorkUsedAsDState(S, 161, 1);
33008
33009 /* '<S89>/Generated S-Function3': DWORK0 */
33010 ssSetDWorkName(S, 162, "DWORK162");
33011 ssSetDWorkWidth(S, 162, 1);
33012 ssSetDWorkDataType(S, 162, SS_SINGLE);
33013 ssSetDWorkUsedAsDState(S, 162, 1);
33014
33015 /* '<S89>/Generated S-Function3': DWORK1 */
33016 ssSetDWorkName(S, 163, "DWORK163");
33017 ssSetDWorkWidth(S, 163, 1);
33018 ssSetDWorkDataType(S, 163, SS_SINGLE);
33019 ssSetDWorkUsedAsDState(S, 163, 1);
33020
33021 /* '<S90>/Generated S-Function2': DWORK0 */
33022 ssSetDWorkName(S, 164, "DWORK164");
33023 ssSetDWorkWidth(S, 164, 1);
33024 ssSetDWorkDataType(S, 164, SS_SINGLE);
33025 ssSetDWorkUsedAsDState(S, 164, 1);
33026
33027 /* '<S90>/Generated S-Function2': DWORK1 */
33028 ssSetDWorkName(S, 165, "DWORK165");
33029 ssSetDWorkWidth(S, 165, 1);
33030 ssSetDWorkDataType(S, 165, SS_SINGLE);
33031 ssSetDWorkUsedAsDState(S, 165, 1);
33032
33033 /* '<S87>/Generated S-Function4': DWORK0 */
33034 ssSetDWorkName(S, 166, "DWORK166");
33035 ssSetDWorkWidth(S, 166, 1);
33036 ssSetDWorkDataType(S, 166, SS_SINGLE);
33037 ssSetDWorkUsedAsDState(S, 166, 1);
33038
33039 /* '<S87>/Generated S-Function4': DWORK1 */
33040 ssSetDWorkName(S, 167, "DWORK167");
33041 ssSetDWorkWidth(S, 167, 1);
33042 ssSetDWorkDataType(S, 167, SS_SINGLE);
33043 ssSetDWorkUsedAsDState(S, 167, 1);
33044
33045 /* '<S113>/Generated S-Function': DWORK0 */
33046 ssSetDWorkName(S, 168, "DWORK168");
33047 ssSetDWorkWidth(S, 168, 1);
33048 ssSetDWorkDataType(S, 168, SS_SINGLE);
33049 ssSetDWorkUsedAsDState(S, 168, 1);
33050
33051 /* '<S113>/Generated S-Function': DWORK1 */
33052 ssSetDWorkName(S, 169, "DWORK169");
33053 ssSetDWorkWidth(S, 169, 1);
33054 ssSetDWorkDataType(S, 169, SS_SINGLE);
33055 ssSetDWorkUsedAsDState(S, 169, 1);
33056
33057 /* '<S118>/Generated S-Function2': DWORK0 */
33058 ssSetDWorkName(S, 170, "DWORK170");
33059 ssSetDWorkWidth(S, 170, 1);
33060 ssSetDWorkDataType(S, 170, SS_SINGLE);
33061 ssSetDWorkUsedAsDState(S, 170, 1);
33062
33063 /* '<S118>/Generated S-Function2': DWORK1 */
33064 ssSetDWorkName(S, 171, "DWORK171");
33065 ssSetDWorkWidth(S, 171, 1);
33066 ssSetDWorkDataType(S, 171, SS_SINGLE);
33067 ssSetDWorkUsedAsDState(S, 171, 1);
33068
33069 /* '<S117>/Generated S-Function4': DWORK0 */
33070 ssSetDWorkName(S, 172, "DWORK172");
33071 ssSetDWorkWidth(S, 172, 1);
33072 ssSetDWorkDataType(S, 172, SS_SINGLE);
33073 ssSetDWorkUsedAsDState(S, 172, 1);
33074
33075 /* '<S117>/Generated S-Function4': DWORK1 */
33076 ssSetDWorkName(S, 173, "DWORK173");
33077 ssSetDWorkWidth(S, 173, 1);
33078 ssSetDWorkDataType(S, 173, SS_SINGLE);
33079 ssSetDWorkUsedAsDState(S, 173, 1);
33080
33081 /* '<S116>/Generated S-Function4': DWORK0 */
33082 ssSetDWorkName(S, 174, "DWORK174");
33083 ssSetDWorkWidth(S, 174, 1);
33084 ssSetDWorkDataType(S, 174, SS_SINGLE);
33085 ssSetDWorkUsedAsDState(S, 174, 1);
33086
33087 /* '<S116>/Generated S-Function4': DWORK1 */
33088 ssSetDWorkName(S, 175, "DWORK175");
33089 ssSetDWorkWidth(S, 175, 1);
33090 ssSetDWorkDataType(S, 175, SS_SINGLE);
33091 ssSetDWorkUsedAsDState(S, 175, 1);
33092
33093 /* '<S100>/Generated S-Function3': DWORK0 */
33094 ssSetDWorkName(S, 176, "DWORK176");
33095 ssSetDWorkWidth(S, 176, 1);
33096 ssSetDWorkDataType(S, 176, SS_SINGLE);
33097 ssSetDWorkUsedAsDState(S, 176, 1);
33098
33099 /* '<S100>/Generated S-Function3': DWORK1 */
33100 ssSetDWorkName(S, 177, "DWORK177");
33101 ssSetDWorkWidth(S, 177, 1);
33102 ssSetDWorkDataType(S, 177, SS_SINGLE);
33103 ssSetDWorkUsedAsDState(S, 177, 1);
33104
33105 /* '<S101>/Generated S-Function2': DWORK0 */
33106 ssSetDWorkName(S, 178, "DWORK178");
33107 ssSetDWorkWidth(S, 178, 1);
33108 ssSetDWorkDataType(S, 178, SS_SINGLE);
33109 ssSetDWorkUsedAsDState(S, 178, 1);
33110
33111 /* '<S101>/Generated S-Function2': DWORK1 */
33112 ssSetDWorkName(S, 179, "DWORK179");
33113 ssSetDWorkWidth(S, 179, 1);
33114 ssSetDWorkDataType(S, 179, SS_SINGLE);
33115 ssSetDWorkUsedAsDState(S, 179, 1);
33116
33117 /* '<S98>/Generated S-Function4': DWORK0 */
33118 ssSetDWorkName(S, 180, "DWORK180");
33119 ssSetDWorkWidth(S, 180, 1);
33120 ssSetDWorkDataType(S, 180, SS_SINGLE);
33121 ssSetDWorkUsedAsDState(S, 180, 1);
33122
33123 /* '<S98>/Generated S-Function4': DWORK1 */
33124 ssSetDWorkName(S, 181, "DWORK181");
33125 ssSetDWorkWidth(S, 181, 1);
33126 ssSetDWorkDataType(S, 181, SS_SINGLE);
33127 ssSetDWorkUsedAsDState(S, 181, 1);
33128
33129 /* '<S107>/Generated S-Function3': DWORK0 */
33130 ssSetDWorkName(S, 182, "DWORK182");
33131 ssSetDWorkWidth(S, 182, 1);
33132 ssSetDWorkDataType(S, 182, SS_SINGLE);
33133 ssSetDWorkUsedAsDState(S, 182, 1);
33134
33135 /* '<S107>/Generated S-Function3': DWORK1 */
33136 ssSetDWorkName(S, 183, "DWORK183");
33137 ssSetDWorkWidth(S, 183, 1);
33138 ssSetDWorkDataType(S, 183, SS_SINGLE);
33139 ssSetDWorkUsedAsDState(S, 183, 1);
33140
33141 /* '<S108>/Generated S-Function2': DWORK0 */
33142 ssSetDWorkName(S, 184, "DWORK184");
33143 ssSetDWorkWidth(S, 184, 1);
33144 ssSetDWorkDataType(S, 184, SS_SINGLE);
33145 ssSetDWorkUsedAsDState(S, 184, 1);
33146
33147 /* '<S108>/Generated S-Function2': DWORK1 */
33148 ssSetDWorkName(S, 185, "DWORK185");
33149 ssSetDWorkWidth(S, 185, 1);
33150 ssSetDWorkDataType(S, 185, SS_SINGLE);
33151 ssSetDWorkUsedAsDState(S, 185, 1);
33152
33153 /* '<S106>/Generated S-Function4': DWORK0 */
33154 ssSetDWorkName(S, 186, "DWORK186");
33155 ssSetDWorkWidth(S, 186, 1);
33156 ssSetDWorkDataType(S, 186, SS_SINGLE);
33157 ssSetDWorkUsedAsDState(S, 186, 1);
33158
33159 /* '<S106>/Generated S-Function4': DWORK1 */
33160 ssSetDWorkName(S, 187, "DWORK187");
33161 ssSetDWorkWidth(S, 187, 1);
33162 ssSetDWorkDataType(S, 187, SS_SINGLE);
33163 ssSetDWorkUsedAsDState(S, 187, 1);
33164
33165 /* '<S138>/Unit Delay1': DSTATE */
33166 ssSetDWorkName(S, 188, "DWORK188");
33167 ssSetDWorkWidth(S, 188, 1);
33168 ssSetDWorkDataType(S, 188, SS_SINGLE);
33169 ssSetDWorkUsedAsDState(S, 188, 1);
33170
33171 /* '<S130>/Generated S-Function4': DWORK0 */
33172 ssSetDWorkName(S, 189, "DWORK189");
33173 ssSetDWorkWidth(S, 189, 1);
33174 ssSetDWorkDataType(S, 189, SS_SINGLE);
33175 ssSetDWorkUsedAsDState(S, 189, 1);
33176
33177 /* '<S130>/Generated S-Function4': DWORK1 */
33178 ssSetDWorkName(S, 190, "DWORK190");
33179 ssSetDWorkWidth(S, 190, 1);
33180 ssSetDWorkDataType(S, 190, SS_SINGLE);
33181 ssSetDWorkUsedAsDState(S, 190, 1);
33182
33183 /* '<S125>/Unit Delay1': DSTATE */
33184 ssSetDWorkName(S, 191, "DWORK191");
33185 ssSetDWorkWidth(S, 191, 1);
33186 ssSetDWorkDataType(S, 191, SS_SINGLE);
33187 ssSetDWorkUsedAsDState(S, 191, 1);
33188
33189 /* '<S132>/Generated S-Function3': DWORK0 */
33190 ssSetDWorkName(S, 192, "DWORK192");
33191 ssSetDWorkWidth(S, 192, 1);
33192 ssSetDWorkDataType(S, 192, SS_SINGLE);
33193 ssSetDWorkUsedAsDState(S, 192, 1);
33194
33195 /* '<S132>/Generated S-Function3': DWORK1 */
33196 ssSetDWorkName(S, 193, "DWORK193");
33197 ssSetDWorkWidth(S, 193, 1);
33198 ssSetDWorkDataType(S, 193, SS_SINGLE);
33199 ssSetDWorkUsedAsDState(S, 193, 1);
33200
33201 /* '<S133>/Generated S-Function2': DWORK0 */
33202 ssSetDWorkName(S, 194, "DWORK194");
33203 ssSetDWorkWidth(S, 194, 1);
33204 ssSetDWorkDataType(S, 194, SS_SINGLE);
33205 ssSetDWorkUsedAsDState(S, 194, 1);
33206
33207 /* '<S133>/Generated S-Function2': DWORK1 */
33208 ssSetDWorkName(S, 195, "DWORK195");
33209 ssSetDWorkWidth(S, 195, 1);
33210 ssSetDWorkDataType(S, 195, SS_SINGLE);
33211 ssSetDWorkUsedAsDState(S, 195, 1);
33212
33213 /* '<S131>/Generated S-Function4': DWORK0 */
33214 ssSetDWorkName(S, 196, "DWORK196");
33215 ssSetDWorkWidth(S, 196, 1);
33216 ssSetDWorkDataType(S, 196, SS_SINGLE);
33217 ssSetDWorkUsedAsDState(S, 196, 1);
33218
33219 /* '<S131>/Generated S-Function4': DWORK1 */
33220 ssSetDWorkName(S, 197, "DWORK197");
33221 ssSetDWorkWidth(S, 197, 1);
33222 ssSetDWorkDataType(S, 197, SS_SINGLE);
33223 ssSetDWorkUsedAsDState(S, 197, 1);
33224
33225 /* '<S152>/Generated S-Function2': DWORK0 */
33226 ssSetDWorkName(S, 198, "DWORK198");
33227 ssSetDWorkWidth(S, 198, 1);
33228 ssSetDWorkDataType(S, 198, SS_SINGLE);
33229 ssSetDWorkUsedAsDState(S, 198, 1);
33230
33231 /* '<S152>/Generated S-Function2': DWORK1 */
33232 ssSetDWorkName(S, 199, "DWORK199");
33233 ssSetDWorkWidth(S, 199, 1);
33234 ssSetDWorkDataType(S, 199, SS_SINGLE);
33235 ssSetDWorkUsedAsDState(S, 199, 1);
33236
33237 /* '<S150>/Generated S-Function3': DWORK0 */
33238 ssSetDWorkName(S, 200, "DWORK200");
33239 ssSetDWorkWidth(S, 200, 1);
33240 ssSetDWorkDataType(S, 200, SS_SINGLE);
33241 ssSetDWorkUsedAsDState(S, 200, 1);
33242
33243 /* '<S150>/Generated S-Function3': DWORK1 */
33244 ssSetDWorkName(S, 201, "DWORK201");
33245 ssSetDWorkWidth(S, 201, 1);
33246 ssSetDWorkDataType(S, 201, SS_SINGLE);
33247 ssSetDWorkUsedAsDState(S, 201, 1);
33248
33249 /* '<S151>/Generated S-Function2': DWORK0 */
33250 ssSetDWorkName(S, 202, "DWORK202");
33251 ssSetDWorkWidth(S, 202, 1);
33252 ssSetDWorkDataType(S, 202, SS_SINGLE);
33253 ssSetDWorkUsedAsDState(S, 202, 1);
33254
33255 /* '<S151>/Generated S-Function2': DWORK1 */
33256 ssSetDWorkName(S, 203, "DWORK203");
33257 ssSetDWorkWidth(S, 203, 1);
33258 ssSetDWorkDataType(S, 203, SS_SINGLE);
33259 ssSetDWorkUsedAsDState(S, 203, 1);
33260
33261 /* '<S149>/Generated S-Function4': DWORK0 */
33262 ssSetDWorkName(S, 204, "DWORK204");
33263 ssSetDWorkWidth(S, 204, 1);
33264 ssSetDWorkDataType(S, 204, SS_SINGLE);
33265 ssSetDWorkUsedAsDState(S, 204, 1);
33266
33267 /* '<S149>/Generated S-Function4': DWORK1 */
33268 ssSetDWorkName(S, 205, "DWORK205");
33269 ssSetDWorkWidth(S, 205, 1);
33270 ssSetDWorkDataType(S, 205, SS_SINGLE);
33271 ssSetDWorkUsedAsDState(S, 205, 1);
33272
33273 /* '<S142>/Generated S-Function2': DWORK0 */
33274 ssSetDWorkName(S, 206, "DWORK206");
33275 ssSetDWorkWidth(S, 206, 1);
33276 ssSetDWorkDataType(S, 206, SS_SINGLE);
33277 ssSetDWorkUsedAsDState(S, 206, 1);
33278
33279 /* '<S142>/Generated S-Function2': DWORK1 */
33280 ssSetDWorkName(S, 207, "DWORK207");
33281 ssSetDWorkWidth(S, 207, 1);
33282 ssSetDWorkDataType(S, 207, SS_SINGLE);
33283 ssSetDWorkUsedAsDState(S, 207, 1);
33284
33285 /* '<S140>/Generated S-Function3': DWORK0 */
33286 ssSetDWorkName(S, 208, "DWORK208");
33287 ssSetDWorkWidth(S, 208, 1);
33288 ssSetDWorkDataType(S, 208, SS_SINGLE);
33289 ssSetDWorkUsedAsDState(S, 208, 1);
33290
33291 /* '<S140>/Generated S-Function3': DWORK1 */
33292 ssSetDWorkName(S, 209, "DWORK209");
33293 ssSetDWorkWidth(S, 209, 1);
33294 ssSetDWorkDataType(S, 209, SS_SINGLE);
33295 ssSetDWorkUsedAsDState(S, 209, 1);
33296
33297 /* '<S141>/Generated S-Function2': DWORK0 */
33298 ssSetDWorkName(S, 210, "DWORK210");
33299 ssSetDWorkWidth(S, 210, 1);
33300 ssSetDWorkDataType(S, 210, SS_SINGLE);
33301 ssSetDWorkUsedAsDState(S, 210, 1);
33302
33303 /* '<S141>/Generated S-Function2': DWORK1 */
33304 ssSetDWorkName(S, 211, "DWORK211");
33305 ssSetDWorkWidth(S, 211, 1);
33306 ssSetDWorkDataType(S, 211, SS_SINGLE);
33307 ssSetDWorkUsedAsDState(S, 211, 1);
33308
33309 /* '<S139>/Generated S-Function4': DWORK0 */
33310 ssSetDWorkName(S, 212, "DWORK212");
33311 ssSetDWorkWidth(S, 212, 1);
33312 ssSetDWorkDataType(S, 212, SS_SINGLE);
33313 ssSetDWorkUsedAsDState(S, 212, 1);
33314
33315 /* '<S139>/Generated S-Function4': DWORK1 */
33316 ssSetDWorkName(S, 213, "DWORK213");
33317 ssSetDWorkWidth(S, 213, 1);
33318 ssSetDWorkDataType(S, 213, SS_SINGLE);
33319 ssSetDWorkUsedAsDState(S, 213, 1);
33320
33321 /* '<S160>/Generated S-Function3': DWORK0 */
33322 ssSetDWorkName(S, 214, "DWORK214");
33323 ssSetDWorkWidth(S, 214, 1);
33324 ssSetDWorkDataType(S, 214, SS_SINGLE);
33325 ssSetDWorkUsedAsDState(S, 214, 1);
33326
33327 /* '<S160>/Generated S-Function3': DWORK1 */
33328 ssSetDWorkName(S, 215, "DWORK215");
33329 ssSetDWorkWidth(S, 215, 1);
33330 ssSetDWorkDataType(S, 215, SS_SINGLE);
33331 ssSetDWorkUsedAsDState(S, 215, 1);
33332
33333 /* '<S159>/Generated S-Function4': DWORK0 */
33334 ssSetDWorkName(S, 216, "DWORK216");
33335 ssSetDWorkWidth(S, 216, 1);
33336 ssSetDWorkDataType(S, 216, SS_SINGLE);
33337 ssSetDWorkUsedAsDState(S, 216, 1);
33338
33339 /* '<S159>/Generated S-Function4': DWORK1 */
33340 ssSetDWorkName(S, 217, "DWORK217");
33341 ssSetDWorkWidth(S, 217, 1);
33342 ssSetDWorkDataType(S, 217, SS_SINGLE);
33343 ssSetDWorkUsedAsDState(S, 217, 1);
33344
33345 /* '<S195>/Unit Delay1': DSTATE */
33346 ssSetDWorkName(S, 218, "DWORK218");
33347 ssSetDWorkWidth(S, 218, 1);
33348 ssSetDWorkDataType(S, 218, SS_SINGLE);
33349 ssSetDWorkUsedAsDState(S, 218, 1);
33350
33351 /* '<S190>/Generated S-Function4': DWORK0 */
33352 ssSetDWorkName(S, 219, "DWORK219");
33353 ssSetDWorkWidth(S, 219, 1);
33354 ssSetDWorkDataType(S, 219, SS_SINGLE);
33355 ssSetDWorkUsedAsDState(S, 219, 1);
33356
33357 /* '<S190>/Generated S-Function4': DWORK1 */
33358 ssSetDWorkName(S, 220, "DWORK220");
33359 ssSetDWorkWidth(S, 220, 1);
33360 ssSetDWorkDataType(S, 220, SS_SINGLE);
33361 ssSetDWorkUsedAsDState(S, 220, 1);
33362
33363 /* '<S191>/Generated S-Function4': DWORK0 */
33364 ssSetDWorkName(S, 221, "DWORK221");
33365 ssSetDWorkWidth(S, 221, 1);
33366 ssSetDWorkDataType(S, 221, SS_SINGLE);
33367 ssSetDWorkUsedAsDState(S, 221, 1);
33368
33369 /* '<S191>/Generated S-Function4': DWORK1 */
33370 ssSetDWorkName(S, 222, "DWORK222");
33371 ssSetDWorkWidth(S, 222, 1);
33372 ssSetDWorkDataType(S, 222, SS_SINGLE);
33373 ssSetDWorkUsedAsDState(S, 222, 1);
33374
33375 /* '<S197>/Generated S-Function2': DWORK0 */
33376 ssSetDWorkName(S, 223, "DWORK223");
33377 ssSetDWorkWidth(S, 223, 1);
33378 ssSetDWorkDataType(S, 223, SS_SINGLE);
33379 ssSetDWorkUsedAsDState(S, 223, 1);
33380
33381 /* '<S197>/Generated S-Function2': DWORK1 */
33382 ssSetDWorkName(S, 224, "DWORK224");
33383 ssSetDWorkWidth(S, 224, 1);
33384 ssSetDWorkDataType(S, 224, SS_SINGLE);
33385 ssSetDWorkUsedAsDState(S, 224, 1);
33386
33387 /* '<S196>/Generated S-Function4': DWORK0 */
33388 ssSetDWorkName(S, 225, "DWORK225");
33389 ssSetDWorkWidth(S, 225, 1);
33390 ssSetDWorkDataType(S, 225, SS_SINGLE);
33391 ssSetDWorkUsedAsDState(S, 225, 1);
33392
33393 /* '<S196>/Generated S-Function4': DWORK1 */
33394 ssSetDWorkName(S, 226, "DWORK226");
33395 ssSetDWorkWidth(S, 226, 1);
33396 ssSetDWorkDataType(S, 226, SS_SINGLE);
33397 ssSetDWorkUsedAsDState(S, 226, 1);
33398
33399 /* '<S201>/Generated S-Function2': DWORK0 */
33400 ssSetDWorkName(S, 227, "DWORK227");
33401 ssSetDWorkWidth(S, 227, 1);
33402 ssSetDWorkDataType(S, 227, SS_SINGLE);
33403 ssSetDWorkUsedAsDState(S, 227, 1);
33404
33405 /* '<S201>/Generated S-Function2': DWORK1 */
33406 ssSetDWorkName(S, 228, "DWORK228");
33407 ssSetDWorkWidth(S, 228, 1);
33408 ssSetDWorkDataType(S, 228, SS_SINGLE);
33409 ssSetDWorkUsedAsDState(S, 228, 1);
33410
33411 /* '<S200>/Generated S-Function4': DWORK0 */
33412 ssSetDWorkName(S, 229, "DWORK229");
33413 ssSetDWorkWidth(S, 229, 1);
33414 ssSetDWorkDataType(S, 229, SS_SINGLE);
33415 ssSetDWorkUsedAsDState(S, 229, 1);
33416
33417 /* '<S200>/Generated S-Function4': DWORK1 */
33418 ssSetDWorkName(S, 230, "DWORK230");
33419 ssSetDWorkWidth(S, 230, 1);
33420 ssSetDWorkDataType(S, 230, SS_SINGLE);
33421 ssSetDWorkUsedAsDState(S, 230, 1);
33422
33423 /* '<S73>/Unit Delay1': DSTATE */
33424 ssSetDWorkName(S, 231, "DWORK231");
33425 ssSetDWorkWidth(S, 231, 1);
33426 ssSetDWorkDataType(S, 231, SS_SINGLE);
33427 ssSetDWorkUsedAsDState(S, 231, 1);
33428
33429 /* '<S69>/Generated S-Function4': DWORK0 */
33430 ssSetDWorkName(S, 232, "DWORK232");
33431 ssSetDWorkWidth(S, 232, 1);
33432 ssSetDWorkDataType(S, 232, SS_SINGLE);
33433 ssSetDWorkUsedAsDState(S, 232, 1);
33434
33435 /* '<S69>/Generated S-Function4': DWORK1 */
33436 ssSetDWorkName(S, 233, "DWORK233");
33437 ssSetDWorkWidth(S, 233, 1);
33438 ssSetDWorkDataType(S, 233, SS_SINGLE);
33439 ssSetDWorkUsedAsDState(S, 233, 1);
33440
33441 /* '<S70>/Generated S-Function4': DWORK0 */
33442 ssSetDWorkName(S, 234, "DWORK234");
33443 ssSetDWorkWidth(S, 234, 1);
33444 ssSetDWorkDataType(S, 234, SS_SINGLE);
33445 ssSetDWorkUsedAsDState(S, 234, 1);
33446
33447 /* '<S70>/Generated S-Function4': DWORK1 */
33448 ssSetDWorkName(S, 235, "DWORK235");
33449 ssSetDWorkWidth(S, 235, 1);
33450 ssSetDWorkDataType(S, 235, SS_SINGLE);
33451 ssSetDWorkUsedAsDState(S, 235, 1);
33452
33453 /* '<S75>/Generated S-Function2': DWORK0 */
33454 ssSetDWorkName(S, 236, "DWORK236");
33455 ssSetDWorkWidth(S, 236, 1);
33456 ssSetDWorkDataType(S, 236, SS_SINGLE);
33457 ssSetDWorkUsedAsDState(S, 236, 1);
33458
33459 /* '<S75>/Generated S-Function2': DWORK1 */
33460 ssSetDWorkName(S, 237, "DWORK237");
33461 ssSetDWorkWidth(S, 237, 1);
33462 ssSetDWorkDataType(S, 237, SS_SINGLE);
33463 ssSetDWorkUsedAsDState(S, 237, 1);
33464
33465 /* '<S74>/Generated S-Function4': DWORK0 */
33466 ssSetDWorkName(S, 238, "DWORK238");
33467 ssSetDWorkWidth(S, 238, 1);
33468 ssSetDWorkDataType(S, 238, SS_SINGLE);
33469 ssSetDWorkUsedAsDState(S, 238, 1);
33470
33471 /* '<S74>/Generated S-Function4': DWORK1 */
33472 ssSetDWorkName(S, 239, "DWORK239");
33473 ssSetDWorkWidth(S, 239, 1);
33474 ssSetDWorkDataType(S, 239, SS_SINGLE);
33475 ssSetDWorkUsedAsDState(S, 239, 1);
33476
33477 /* '<S208>/Generated S-Function': DWORK0 */
33478 ssSetDWorkName(S, 240, "DWORK240");
33479 ssSetDWorkWidth(S, 240, 1);
33480 ssSetDWorkDataType(S, 240, SS_SINGLE);
33481 ssSetDWorkUsedAsDState(S, 240, 1);
33482
33483 /* '<S241>/Generated S-Function4': DWORK0 */
33484 ssSetDWorkName(S, 241, "DWORK241");
33485 ssSetDWorkWidth(S, 241, 1);
33486 ssSetDWorkDataType(S, 241, SS_SINGLE);
33487 ssSetDWorkUsedAsDState(S, 241, 1);
33488
33489 /* '<S241>/Generated S-Function4': DWORK1 */
33490 ssSetDWorkName(S, 242, "DWORK242");
33491 ssSetDWorkWidth(S, 242, 1);
33492 ssSetDWorkDataType(S, 242, SS_SINGLE);
33493 ssSetDWorkUsedAsDState(S, 242, 1);
33494
33495 /* '<S239>/Generated S-Function4': DWORK0 */
33496 ssSetDWorkName(S, 243, "DWORK243");
33497 ssSetDWorkWidth(S, 243, 1);
33498 ssSetDWorkDataType(S, 243, SS_SINGLE);
33499 ssSetDWorkUsedAsDState(S, 243, 1);
33500
33501 /* '<S239>/Generated S-Function4': DWORK1 */
33502 ssSetDWorkName(S, 244, "DWORK244");
33503 ssSetDWorkWidth(S, 244, 1);
33504 ssSetDWorkDataType(S, 244, SS_SINGLE);
33505 ssSetDWorkUsedAsDState(S, 244, 1);
33506
33507 /* '<S240>/Generated S-Function4': DWORK0 */
33508 ssSetDWorkName(S, 245, "DWORK245");
33509 ssSetDWorkWidth(S, 245, 1);
33510 ssSetDWorkDataType(S, 245, SS_SINGLE);
33511 ssSetDWorkUsedAsDState(S, 245, 1);
33512
33513 /* '<S240>/Generated S-Function4': DWORK1 */
33514 ssSetDWorkName(S, 246, "DWORK246");
33515 ssSetDWorkWidth(S, 246, 1);
33516 ssSetDWorkDataType(S, 246, SS_SINGLE);
33517 ssSetDWorkUsedAsDState(S, 246, 1);
33518
33519 /* '<S238>/Generated S-Function8': DWORK0 */
33520 ssSetDWorkName(S, 247, "DWORK247");
33521 ssSetDWorkWidth(S, 247, 1);
33522 ssSetDWorkDataType(S, 247, SS_SINGLE);
33523 ssSetDWorkUsedAsDState(S, 247, 1);
33524
33525 /* '<S238>/Generated S-Function8': DWORK1 */
33526 ssSetDWorkName(S, 248, "DWORK248");
33527 ssSetDWorkWidth(S, 248, 1);
33528 ssSetDWorkDataType(S, 248, SS_SINGLE);
33529 ssSetDWorkUsedAsDState(S, 248, 1);
33530
33531 /* '<S223>/Generated S-Function7': DWORK0 */
33532 ssSetDWorkName(S, 249, "DWORK249");
33533 ssSetDWorkWidth(S, 249, 1);
33534 ssSetDWorkDataType(S, 249, SS_SINGLE);
33535 ssSetDWorkUsedAsDState(S, 249, 1);
33536
33537 /* '<S224>/Generated S-Function7': DWORK0 */
33538 ssSetDWorkName(S, 250, "DWORK250");
33539 ssSetDWorkWidth(S, 250, 1);
33540 ssSetDWorkDataType(S, 250, SS_SINGLE);
33541 ssSetDWorkUsedAsDState(S, 250, 1);
33542
33543 /* '<S252>/Generated S-Function8': DWORK0 */
33544 ssSetDWorkName(S, 251, "DWORK251");
33545 ssSetDWorkWidth(S, 251, 1);
33546 ssSetDWorkDataType(S, 251, SS_SINGLE);
33547 ssSetDWorkUsedAsDState(S, 251, 1);
33548
33549 /* '<S252>/Generated S-Function8': DWORK1 */
33550 ssSetDWorkName(S, 252, "DWORK252");
33551 ssSetDWorkWidth(S, 252, 1);
33552 ssSetDWorkDataType(S, 252, SS_SINGLE);
33553 ssSetDWorkUsedAsDState(S, 252, 1);
33554
33555 /* '<S253>/Generated S-Function4': DWORK0 */
33556 ssSetDWorkName(S, 253, "DWORK253");
33557 ssSetDWorkWidth(S, 253, 1);
33558 ssSetDWorkDataType(S, 253, SS_SINGLE);
33559 ssSetDWorkUsedAsDState(S, 253, 1);
33560
33561 /* '<S253>/Generated S-Function4': DWORK1 */
33562 ssSetDWorkName(S, 254, "DWORK254");
33563 ssSetDWorkWidth(S, 254, 1);
33564 ssSetDWorkDataType(S, 254, SS_SINGLE);
33565 ssSetDWorkUsedAsDState(S, 254, 1);
33566
33567 /* '<S254>/Generated S-Function4': DWORK0 */
33568 ssSetDWorkName(S, 255, "DWORK255");
33569 ssSetDWorkWidth(S, 255, 1);
33570 ssSetDWorkDataType(S, 255, SS_SINGLE);
33571 ssSetDWorkUsedAsDState(S, 255, 1);
33572
33573 /* '<S254>/Generated S-Function4': DWORK1 */
33574 ssSetDWorkName(S, 256, "DWORK256");
33575 ssSetDWorkWidth(S, 256, 1);
33576 ssSetDWorkDataType(S, 256, SS_SINGLE);
33577 ssSetDWorkUsedAsDState(S, 256, 1);
33578
33579 /* '<S266>/Generated S-Function': DWORK0 */
33580 ssSetDWorkName(S, 257, "DWORK257");
33581 ssSetDWorkWidth(S, 257, 1);
33582 ssSetDWorkDataType(S, 257, SS_SINGLE);
33583 ssSetDWorkUsedAsDState(S, 257, 1);
33584
33585 /* '<S266>/Generated S-Function': DWORK1 */
33586 ssSetDWorkName(S, 258, "DWORK258");
33587 ssSetDWorkWidth(S, 258, 1);
33588 ssSetDWorkDataType(S, 258, SS_SINGLE);
33589 ssSetDWorkUsedAsDState(S, 258, 1);
33590
33591 /* '<S265>/Generated S-Function8': DWORK0 */
33592 ssSetDWorkName(S, 259, "DWORK259");
33593 ssSetDWorkWidth(S, 259, 1);
33594 ssSetDWorkDataType(S, 259, SS_SINGLE);
33595 ssSetDWorkUsedAsDState(S, 259, 1);
33596
33597 /* '<S265>/Generated S-Function8': DWORK1 */
33598 ssSetDWorkName(S, 260, "DWORK260");
33599 ssSetDWorkWidth(S, 260, 1);
33600 ssSetDWorkDataType(S, 260, SS_SINGLE);
33601 ssSetDWorkUsedAsDState(S, 260, 1);
33602
33603 /* '<S273>/Unit Delay1': DSTATE */
33604 ssSetDWorkName(S, 261, "DWORK261");
33605 ssSetDWorkWidth(S, 261, 1);
33606 ssSetDWorkDataType(S, 261, SS_SINGLE);
33607 ssSetDWorkUsedAsDState(S, 261, 1);
33608
33609 /* '<S247>/Unit Delay1': DSTATE */
33610 ssSetDWorkName(S, 262, "DWORK262");
33611 ssSetDWorkWidth(S, 262, 1);
33612 ssSetDWorkDataType(S, 262, SS_SINGLE);
33613 ssSetDWorkUsedAsDState(S, 262, 1);
33614
33615 /* '<S257>/Generated S-Function3': DWORK0 */
33616 ssSetDWorkName(S, 263, "DWORK263");
33617 ssSetDWorkWidth(S, 263, 1);
33618 ssSetDWorkDataType(S, 263, SS_SINGLE);
33619 ssSetDWorkUsedAsDState(S, 263, 1);
33620
33621 /* '<S257>/Generated S-Function3': DWORK1 */
33622 ssSetDWorkName(S, 264, "DWORK264");
33623 ssSetDWorkWidth(S, 264, 1);
33624 ssSetDWorkDataType(S, 264, SS_SINGLE);
33625 ssSetDWorkUsedAsDState(S, 264, 1);
33626
33627 /* '<S249>/Generated S-Function7': DWORK0 */
33628 ssSetDWorkName(S, 265, "DWORK265");
33629 ssSetDWorkWidth(S, 265, 1);
33630 ssSetDWorkDataType(S, 265, SS_SINGLE);
33631 ssSetDWorkUsedAsDState(S, 265, 1);
33632
33633 /* '<S274>/Generated S-Function7': DWORK0 */
33634 ssSetDWorkName(S, 266, "DWORK266");
33635 ssSetDWorkWidth(S, 266, 1);
33636 ssSetDWorkDataType(S, 266, SS_SINGLE);
33637 ssSetDWorkUsedAsDState(S, 266, 1);
33638
33639 /* '<S215>/Generated S-Function4': DWORK0 */
33640 ssSetDWorkName(S, 267, "DWORK267");
33641 ssSetDWorkWidth(S, 267, 1);
33642 ssSetDWorkDataType(S, 267, SS_SINGLE);
33643 ssSetDWorkUsedAsDState(S, 267, 1);
33644
33645 /* '<S215>/Generated S-Function4': DWORK1 */
33646 ssSetDWorkName(S, 268, "DWORK268");
33647 ssSetDWorkWidth(S, 268, 1);
33648 ssSetDWorkDataType(S, 268, SS_SINGLE);
33649 ssSetDWorkUsedAsDState(S, 268, 1);
33650
33651 /* '<S216>/Generated S-Function7': DWORK0 */
33652 ssSetDWorkName(S, 269, "DWORK269");
33653 ssSetDWorkWidth(S, 269, 1);
33654 ssSetDWorkDataType(S, 269, SS_SINGLE);
33655 ssSetDWorkUsedAsDState(S, 269, 1);
33656
33657 /* '<S433>/Memory10': PreviousInput */
33658 ssSetDWorkName(S, 270, "DWORK270");
33659 ssSetDWorkWidth(S, 270, 1);
33660 ssSetDWorkDataType(S, 270, SS_SINGLE);
33661
33662 /* '<S438>/Memory1': PreviousInput */
33663 ssSetDWorkName(S, 271, "DWORK271");
33664 ssSetDWorkWidth(S, 271, 1);
33665 ssSetDWorkDataType(S, 271, SS_SINGLE);
33666
33667 /* '<S437>/Memory1': PreviousInput */
33668 ssSetDWorkName(S, 272, "DWORK272");
33669 ssSetDWorkWidth(S, 272, 1);
33670 ssSetDWorkDataType(S, 272, SS_SINGLE);
33671
33672 /* '<S436>/Memory1': PreviousInput */
33673 ssSetDWorkName(S, 273, "DWORK273");
33674 ssSetDWorkWidth(S, 273, 1);
33675 ssSetDWorkDataType(S, 273, SS_SINGLE);
33676
33677 /* '<S435>/Memory1': PreviousInput */
33678 ssSetDWorkName(S, 274, "DWORK274");
33679 ssSetDWorkWidth(S, 274, 1);
33680 ssSetDWorkDataType(S, 274, SS_SINGLE);
33681
33682 /* '<S434>/Memory1': PreviousInput */
33683 ssSetDWorkName(S, 275, "DWORK275");
33684 ssSetDWorkWidth(S, 275, 1);
33685 ssSetDWorkDataType(S, 275, SS_SINGLE);
33686
33687 /* '<S439>/Memory1': PreviousInput */
33688 ssSetDWorkName(S, 276, "DWORK276");
33689 ssSetDWorkWidth(S, 276, 1);
33690 ssSetDWorkDataType(S, 276, SS_SINGLE);
33691
33692 /* '<S433>/Memory1': PreviousInput */
33693 ssSetDWorkName(S, 277, "DWORK277");
33694 ssSetDWorkWidth(S, 277, 1);
33695 ssSetDWorkDataType(S, 277, SS_SINGLE);
33696
33697 /* '<S433>/Memory2': PreviousInput */
33698 ssSetDWorkName(S, 278, "DWORK278");
33699 ssSetDWorkWidth(S, 278, 1);
33700 ssSetDWorkDataType(S, 278, SS_SINGLE);
33701
33702 /* '<S433>/Memory3': PreviousInput */
33703 ssSetDWorkName(S, 279, "DWORK279");
33704 ssSetDWorkWidth(S, 279, 1);
33705 ssSetDWorkDataType(S, 279, SS_SINGLE);
33706
33707 /* '<S433>/Memory4': PreviousInput */
33708 ssSetDWorkName(S, 280, "DWORK280");
33709 ssSetDWorkWidth(S, 280, 1);
33710 ssSetDWorkDataType(S, 280, SS_SINGLE);
33711
33712 /* '<S433>/Memory5': PreviousInput */
33713 ssSetDWorkName(S, 281, "DWORK281");
33714 ssSetDWorkWidth(S, 281, 1);
33715 ssSetDWorkDataType(S, 281, SS_SINGLE);
33716
33717 /* '<S433>/Memory6': PreviousInput */
33718 ssSetDWorkName(S, 282, "DWORK282");
33719 ssSetDWorkWidth(S, 282, 1);
33720 ssSetDWorkDataType(S, 282, SS_SINGLE);
33721
33722 /* '<S433>/Memory7': PreviousInput */
33723 ssSetDWorkName(S, 283, "DWORK283");
33724 ssSetDWorkWidth(S, 283, 1);
33725 ssSetDWorkDataType(S, 283, SS_SINGLE);
33726
33727 /* '<S433>/Memory8': PreviousInput */
33728 ssSetDWorkName(S, 284, "DWORK284");
33729 ssSetDWorkWidth(S, 284, 1);
33730 ssSetDWorkDataType(S, 284, SS_SINGLE);
33731
33732 /* '<S433>/Memory9': PreviousInput */
33733 ssSetDWorkName(S, 285, "DWORK285");
33734 ssSetDWorkWidth(S, 285, 1);
33735 ssSetDWorkDataType(S, 285, SS_SINGLE);
33736
33737 /* '<S316>/Landing2': DWORK21 */
33738 ssSetDWorkName(S, 286, "DWORK286");
33739 ssSetDWorkWidth(S, 286, 1);
33740 ssSetDWorkDataType(S, 286, SS_SINGLE);
33741
33742 /* '<S293>/CompTP': DWORK1 */
33743 ssSetDWorkName(S, 287, "DWORK287");
33744 ssSetDWorkWidth(S, 287, 1);
33745 ssSetDWorkDataType(S, 287, SS_SINGLE);
33746
33747 /* '<S293>/CompTP': DWORK2 */
33748 ssSetDWorkName(S, 288, "DWORK288");
33749 ssSetDWorkWidth(S, 288, 1);
33750 ssSetDWorkDataType(S, 288, SS_SINGLE);
33751
33752 /* '<S81>/Generated S-Function5': DWORK0 */
33753 ssSetDWorkName(S, 289, "DWORK289");
33754 ssSetDWorkWidth(S, 289, 1);
33755 ssSetDWorkDataType(S, 289, SS_SINGLE);
33756
33757 /* '<S81>/Generated S-Function5': DWORK1 */
33758 ssSetDWorkName(S, 290, "DWORK290");
33759 ssSetDWorkWidth(S, 290, 1);
33760 ssSetDWorkDataType(S, 290, SS_SINGLE);
33761
33762 /* '<S81>/Generated S-Function5': DWORK2 */
33763 ssSetDWorkName(S, 291, "DWORK291");
33764 ssSetDWorkWidth(S, 291, 1);
33765 ssSetDWorkDataType(S, 291, SS_SINGLE);
33766
33767 /* '<S123>/Generated S-Function5': DWORK0 */
33768 ssSetDWorkName(S, 292, "DWORK292");
33769 ssSetDWorkWidth(S, 292, 1);
33770 ssSetDWorkDataType(S, 292, SS_SINGLE);
33771
33772 /* '<S123>/Generated S-Function5': DWORK1 */
33773 ssSetDWorkName(S, 293, "DWORK293");
33774 ssSetDWorkWidth(S, 293, 1);
33775 ssSetDWorkDataType(S, 293, SS_SINGLE);
33776
33777 /* '<S123>/Generated S-Function5': DWORK2 */
33778 ssSetDWorkName(S, 294, "DWORK294");
33779 ssSetDWorkWidth(S, 294, 1);
33780 ssSetDWorkDataType(S, 294, SS_SINGLE);
33781
33782 /* '<S12>/LLACalibration2': DWORK4 */
33783 ssSetDWorkName(S, 295, "DWORK295");
33784 ssSetDWorkWidth(S, 295, 1);
33785 ssSetDWorkDataType(S, 295, SS_INT32);
33786
33787 /* '<S32>/wowcmp1': DWORK4 */
33788 ssSetDWorkName(S, 296, "DWORK296");
33789 ssSetDWorkWidth(S, 296, 1);
33790 ssSetDWorkDataType(S, 296, SS_INT32);
33791
33792 /* '<S322>/Taxi': DWORK6 */
33793 ssSetDWorkName(S, 297, "DWORK297");
33794 ssSetDWorkWidth(S, 297, 1);
33795 ssSetDWorkDataType(S, 297, SS_INT32);
33796
33797 /* '<S322>/Taxi': DWORK7 */
33798 ssSetDWorkName(S, 298, "DWORK298");
33799 ssSetDWorkWidth(S, 298, 1);
33800 ssSetDWorkDataType(S, 298, SS_INT32);
33801
33802 /* '<S322>/Taxi': DWORK8 */
33803 ssSetDWorkName(S, 299, "DWORK299");
33804 ssSetDWorkWidth(S, 299, 1);
33805 ssSetDWorkDataType(S, 299, SS_INT32);
33806
33807 /* '<S322>/Taxi': DWORK9 */
33808 ssSetDWorkName(S, 300, "DWORK300");
33809 ssSetDWorkWidth(S, 300, 1);
33810 ssSetDWorkDataType(S, 300, SS_INT32);
33811
33812 /* '<S316>/Landing2': DWORK16 */
33813 ssSetDWorkName(S, 301, "DWORK301");
33814 ssSetDWorkWidth(S, 301, 1);
33815 ssSetDWorkDataType(S, 301, SS_INT32);
33816
33817 /* '<S316>/Landing2': DWORK17 */
33818 ssSetDWorkName(S, 302, "DWORK302");
33819 ssSetDWorkWidth(S, 302, 1);
33820 ssSetDWorkDataType(S, 302, SS_INT32);
33821
33822 /* '<S316>/Landing2': DWORK18 */
33823 ssSetDWorkName(S, 303, "DWORK303");
33824 ssSetDWorkWidth(S, 303, 1);
33825 ssSetDWorkDataType(S, 303, SS_INT32);
33826
33827 /* '<S316>/Landing2': DWORK19 */
33828 ssSetDWorkName(S, 304, "DWORK304");
33829 ssSetDWorkWidth(S, 304, 1);
33830 ssSetDWorkDataType(S, 304, SS_INT32);
33831
33832 /* '<S316>/Landing2': DWORK20 */
33833 ssSetDWorkName(S, 305, "DWORK305");
33834 ssSetDWorkWidth(S, 305, 1);
33835 ssSetDWorkDataType(S, 305, SS_INT32);
33836
33837 /* '<S316>/Landing2': DWORK22 */
33838 ssSetDWorkName(S, 306, "DWORK306");
33839 ssSetDWorkWidth(S, 306, 1);
33840 ssSetDWorkDataType(S, 306, SS_INT32);
33841
33842 /* '<S316>/Landing2': DWORK23 */
33843 ssSetDWorkName(S, 307, "DWORK307");
33844 ssSetDWorkWidth(S, 307, 1);
33845 ssSetDWorkDataType(S, 307, SS_INT32);
33846
33847 /* '<S316>/Landing2': DWORK24 */
33848 ssSetDWorkName(S, 308, "DWORK308");
33849 ssSetDWorkWidth(S, 308, 1);
33850 ssSetDWorkDataType(S, 308, SS_INT32);
33851
33852 /* '<S316>/Landing2': DWORK26 */
33853 ssSetDWorkName(S, 309, "DWORK309");
33854 ssSetDWorkWidth(S, 309, 1);
33855 ssSetDWorkDataType(S, 309, SS_INT32);
33856
33857 /* '<S316>/Landing2': DWORK27 */
33858 ssSetDWorkName(S, 310, "DWORK310");
33859 ssSetDWorkWidth(S, 310, 1);
33860 ssSetDWorkDataType(S, 310, SS_INT32);
33861
33862 /* '<S316>/Landing2': DWORK28 */
33863 ssSetDWorkName(S, 311, "DWORK311");
33864 ssSetDWorkWidth(S, 311, 1);
33865 ssSetDWorkDataType(S, 311, SS_INT32);
33866
33867 /* '<S316>/Landing2': DWORK29 */
33868 ssSetDWorkName(S, 312, "DWORK312");
33869 ssSetDWorkWidth(S, 312, 1);
33870 ssSetDWorkDataType(S, 312, SS_INT32);
33871
33872 /* '<S313>/Approach2': DWORK11 */
33873 ssSetDWorkName(S, 313, "DWORK313");
33874 ssSetDWorkWidth(S, 313, 1);
33875 ssSetDWorkDataType(S, 313, SS_INT32);
33876
33877 /* '<S313>/Approach2': DWORK12 */
33878 ssSetDWorkName(S, 314, "DWORK314");
33879 ssSetDWorkWidth(S, 314, 1);
33880 ssSetDWorkDataType(S, 314, SS_INT32);
33881
33882 /* '<S313>/Approach2': DWORK13 */
33883 ssSetDWorkName(S, 315, "DWORK315");
33884 ssSetDWorkWidth(S, 315, 1);
33885 ssSetDWorkDataType(S, 315, SS_INT32);
33886
33887 /* '<S313>/Approach2': DWORK14 */
33888 ssSetDWorkName(S, 316, "DWORK316");
33889 ssSetDWorkWidth(S, 316, 1);
33890 ssSetDWorkDataType(S, 316, SS_INT32);
33891
33892 /* '<S313>/Approach2': DWORK16 */
33893 ssSetDWorkName(S, 317, "DWORK317");
33894 ssSetDWorkWidth(S, 317, 1);
33895 ssSetDWorkDataType(S, 317, SS_INT32);
33896
33897 /* '<S313>/Approach2': DWORK17 */
33898 ssSetDWorkName(S, 318, "DWORK318");
33899 ssSetDWorkWidth(S, 318, 1);
33900 ssSetDWorkDataType(S, 318, SS_INT32);
33901
33902 /* '<S315>/Cruise': DWORK11 */
33903 ssSetDWorkName(S, 319, "DWORK319");
33904 ssSetDWorkWidth(S, 319, 1);
33905 ssSetDWorkDataType(S, 319, SS_INT32);
33906
33907 /* '<S314>/Climb2': DWORK10 */
33908 ssSetDWorkName(S, 320, "DWORK320");
33909 ssSetDWorkWidth(S, 320, 1);
33910 ssSetDWorkDataType(S, 320, SS_INT32);
33911
33912 /* '<S314>/Climb2': DWORK11 */
33913 ssSetDWorkName(S, 321, "DWORK321");
33914 ssSetDWorkWidth(S, 321, 1);
33915 ssSetDWorkDataType(S, 321, SS_INT32);
33916
33917 /* '<S321>/Takeoff': DWORK5 */
33918 ssSetDWorkName(S, 322, "DWORK322");
33919 ssSetDWorkWidth(S, 322, 1);
33920 ssSetDWorkDataType(S, 322, SS_INT32);
33921
33922 /* '<S321>/Takeoff': DWORK6 */
33923 ssSetDWorkName(S, 323, "DWORK323");
33924 ssSetDWorkWidth(S, 323, 1);
33925 ssSetDWorkDataType(S, 323, SS_INT32);
33926
33927 /* '<S321>/Takeoff': DWORK7 */
33928 ssSetDWorkName(S, 324, "DWORK324");
33929 ssSetDWorkWidth(S, 324, 1);
33930 ssSetDWorkDataType(S, 324, SS_INT32);
33931
33932 /* '<S321>/Takeoff': DWORK8 */
33933 ssSetDWorkName(S, 325, "DWORK325");
33934 ssSetDWorkWidth(S, 325, 1);
33935 ssSetDWorkDataType(S, 325, SS_INT32);
33936
33937 /* '<S321>/Takeoff': DWORK9 */
33938 ssSetDWorkName(S, 326, "DWORK326");
33939 ssSetDWorkWidth(S, 326, 1);
33940 ssSetDWorkDataType(S, 326, SS_INT32);
33941
33942 /* '<S321>/Takeoff': DWORK10 */
33943 ssSetDWorkName(S, 327, "DWORK327");
33944 ssSetDWorkWidth(S, 327, 1);
33945 ssSetDWorkDataType(S, 327, SS_INT32);
33946
33947 /* '<S319>/Pre_Takeoff': DWORK4 */
33948 ssSetDWorkName(S, 328, "DWORK328");
33949 ssSetDWorkWidth(S, 328, 1);
33950 ssSetDWorkDataType(S, 328, SS_INT32);
33951
33952 /* '<S319>/Pre_Takeoff': DWORK5 */
33953 ssSetDWorkName(S, 329, "DWORK329");
33954 ssSetDWorkWidth(S, 329, 1);
33955 ssSetDWorkDataType(S, 329, SS_INT32);
33956
33957 /* '<S319>/Pre_Takeoff': DWORK6 */
33958 ssSetDWorkName(S, 330, "DWORK330");
33959 ssSetDWorkWidth(S, 330, 1);
33960 ssSetDWorkDataType(S, 330, SS_INT32);
33961
33962 /* '<S29>/LLACalibration2': DWORK4 */
33963 ssSetDWorkName(S, 331, "DWORK331");
33964 ssSetDWorkWidth(S, 331, 1);
33965 ssSetDWorkDataType(S, 331, SS_INT32);
33966
33967 /* '<S81>/Generated S-Function5': DWORK3 */
33968 ssSetDWorkName(S, 332, "DWORK332");
33969 ssSetDWorkWidth(S, 332, 1);
33970 ssSetDWorkDataType(S, 332, SS_INT32);
33971
33972 /* '<S81>/Generated S-Function5': DWORK4 */
33973 ssSetDWorkName(S, 333, "DWORK333");
33974 ssSetDWorkWidth(S, 333, 1);
33975 ssSetDWorkDataType(S, 333, SS_INT32);
33976
33977 /* '<S123>/Generated S-Function5': DWORK3 */
33978 ssSetDWorkName(S, 334, "DWORK334");
33979 ssSetDWorkWidth(S, 334, 1);
33980 ssSetDWorkDataType(S, 334, SS_INT32);
33981
33982 /* '<S123>/Generated S-Function5': DWORK4 */
33983 ssSetDWorkName(S, 335, "DWORK335");
33984 ssSetDWorkWidth(S, 335, 1);
33985 ssSetDWorkDataType(S, 335, SS_INT32);
33986
33987 /* '<S21>/delayblock7': DWORK4 */
33988 ssSetDWorkName(S, 336, "DWORK336");
33989 ssSetDWorkWidth(S, 336, 1);
33990 ssSetDWorkDataType(S, 336, SS_INT32);
33991
33992 /* '<S21>/delayblock6': DWORK4 */
33993 ssSetDWorkName(S, 337, "DWORK337");
33994 ssSetDWorkWidth(S, 337, 1);
33995 ssSetDWorkDataType(S, 337, SS_INT32);
33996
33997 /* '<S21>/delayblock5': DWORK4 */
33998 ssSetDWorkName(S, 338, "DWORK338");
33999 ssSetDWorkWidth(S, 338, 1);
34000 ssSetDWorkDataType(S, 338, SS_INT32);
34001
34002 /* '<S21>/delayblock4': DWORK4 */
34003 ssSetDWorkName(S, 339, "DWORK339");
34004 ssSetDWorkWidth(S, 339, 1);
34005 ssSetDWorkDataType(S, 339, SS_INT32);
34006
34007 /* '<S21>/delayblock3': DWORK4 */
34008 ssSetDWorkName(S, 340, "DWORK340");
34009 ssSetDWorkWidth(S, 340, 1);
34010 ssSetDWorkDataType(S, 340, SS_INT32);
34011
34012 /* '<S21>/delayblock2': DWORK4 */
34013 ssSetDWorkName(S, 341, "DWORK341");
34014 ssSetDWorkWidth(S, 341, 1);
34015 ssSetDWorkDataType(S, 341, SS_INT32);
34016
34017 /* '<S21>/delayblock1': DWORK4 */
34018 ssSetDWorkName(S, 342, "DWORK342");
34019 ssSetDWorkWidth(S, 342, 1);
34020 ssSetDWorkDataType(S, 342, SS_INT32);
34021
34022 /* '<S335>/Data Store Memory1': dsmMem */
34023 ssSetDWorkName(S, 343, "DWORK343");
34024 ssSetDWorkWidth(S, 343, 1);
34025 ssSetDWorkDataType(S, 343, SS_UINT32);
34026
34027 /* '<S779>/q': IWORK */
34028 ssSetDWorkName(S, 344, "DWORK344");
34029 ssSetDWorkWidth(S, 344, 1);
34030 ssSetDWorkDataType(S, 344, SS_INTEGER);
34031
34032 /* '<S778>/p1': IWORK */
34033 ssSetDWorkName(S, 345, "DWORK345");
34034 ssSetDWorkWidth(S, 345, 1);
34035 ssSetDWorkDataType(S, 345, SS_INTEGER);
34036
34037 /* '<S780>/ub,vb,wb': IWORK */
34038 ssSetDWorkName(S, 346, "DWORK346");
34039 ssSetDWorkWidth(S, 346, 1);
34040 ssSetDWorkDataType(S, 346, SS_INTEGER);
34041
34042 /* '<S470>/p,q,r ': IWORK */
34043 ssSetDWorkName(S, 347, "DWORK347");
34044 ssSetDWorkWidth(S, 347, 1);
34045 ssSetDWorkDataType(S, 347, SS_INTEGER);
34046
34047 /* '<S683>/Integrator': IWORK */
34048 ssSetDWorkName(S, 348, "DWORK348");
34049 ssSetDWorkWidth(S, 348, 1);
34050 ssSetDWorkDataType(S, 348, SS_INTEGER);
34051
34052 /* '<S682>/Integrator': IWORK */
34053 ssSetDWorkName(S, 349, "DWORK349");
34054 ssSetDWorkWidth(S, 349, 1);
34055 ssSetDWorkDataType(S, 349, SS_INTEGER);
34056
34057 /* '<S778>/p': IWORK */
34058 ssSetDWorkName(S, 350, "DWORK350");
34059 ssSetDWorkWidth(S, 350, 1);
34060 ssSetDWorkDataType(S, 350, SS_INTEGER);
34061
34062 /* '<S335>/Data Store Memory': dsmMem */
34063 ssSetDWorkName(S, 351, "DWORK351");
34064 ssSetDWorkWidth(S, 351, 1);
34065 ssSetDWorkDataType(S, 351, SS_UINT16);
34066
34067 /* '<S24>/Unit Delay3': DSTATE */
34068 ssSetDWorkName(S, 352, "DWORK352");
34069 ssSetDWorkWidth(S, 352, 1);
34070 ssSetDWorkDataType(S, 352, SS_INT8);
34071 ssSetDWorkUsedAsDState(S, 352, 1);
34072
34073 /* '<S7>/Unit Delay': DSTATE */
34074 ssSetDWorkName(S, 353, "DWORK353");
34075 ssSetDWorkWidth(S, 353, 1);
34076 ssSetDWorkDataType(S, 353, SS_INT8);
34077 ssSetDWorkUsedAsDState(S, 353, 1);
34078
34079 /* '<S7>/Unit Delay3': DSTATE */
34080 ssSetDWorkName(S, 354, "DWORK354");
34081 ssSetDWorkWidth(S, 354, 1);
34082 ssSetDWorkDataType(S, 354, SS_INT8);
34083 ssSetDWorkUsedAsDState(S, 354, 1);
34084
34085 /* '<S9>/Unit Delay': DSTATE */
34086 ssSetDWorkName(S, 355, "DWORK355");
34087 ssSetDWorkWidth(S, 355, 1);
34088 ssSetDWorkDataType(S, 355, SS_INT8);
34089 ssSetDWorkUsedAsDState(S, 355, 1);
34090
34091 /* '<S1>/Unit Delay1': DSTATE */
34092 ssSetDWorkName(S, 356, "DWORK356");
34093 ssSetDWorkWidth(S, 356, 1);
34094 ssSetDWorkDataType(S, 356, SS_INT8);
34095 ssSetDWorkUsedAsDState(S, 356, 1);
34096
34097 /* '<S31>/Unit Delay': DSTATE */
34098 ssSetDWorkName(S, 357, "DWORK357");
34099 ssSetDWorkWidth(S, 357, 1);
34100 ssSetDWorkDataType(S, 357, SS_INT8);
34101 ssSetDWorkUsedAsDState(S, 357, 1);
34102
34103 /* '<S31>/Unit Delay1': DSTATE */
34104 ssSetDWorkName(S, 358, "DWORK358");
34105 ssSetDWorkWidth(S, 358, 1);
34106 ssSetDWorkDataType(S, 358, SS_INT8);
34107 ssSetDWorkUsedAsDState(S, 358, 1);
34108
34109 /* '<S31>/Unit Delay2': DSTATE */
34110 ssSetDWorkName(S, 359, "DWORK359");
34111 ssSetDWorkWidth(S, 359, 1);
34112 ssSetDWorkDataType(S, 359, SS_INT8);
34113 ssSetDWorkUsedAsDState(S, 359, 1);
34114
34115 /* '<S31>/Unit Delay3': DSTATE */
34116 ssSetDWorkName(S, 360, "DWORK360");
34117 ssSetDWorkWidth(S, 360, 1);
34118 ssSetDWorkDataType(S, 360, SS_INT8);
34119 ssSetDWorkUsedAsDState(S, 360, 1);
34120
34121 /* '<S31>/Unit Delay4': DSTATE */
34122 ssSetDWorkName(S, 361, "DWORK361");
34123 ssSetDWorkWidth(S, 361, 1);
34124 ssSetDWorkDataType(S, 361, SS_INT8);
34125 ssSetDWorkUsedAsDState(S, 361, 1);
34126
34127 /* '<S31>/Unit Delay7': DSTATE */
34128 ssSetDWorkName(S, 362, "DWORK362");
34129 ssSetDWorkWidth(S, 362, 1);
34130 ssSetDWorkDataType(S, 362, SS_INT8);
34131 ssSetDWorkUsedAsDState(S, 362, 1);
34132
34133 /* '<S31>/Unit Delay5': DSTATE */
34134 ssSetDWorkName(S, 363, "DWORK363");
34135 ssSetDWorkWidth(S, 363, 1);
34136 ssSetDWorkDataType(S, 363, SS_INT8);
34137 ssSetDWorkUsedAsDState(S, 363, 1);
34138
34139 /* '<S32>/Unit Delay3': DSTATE */
34140 ssSetDWorkName(S, 364, "DWORK364");
34141 ssSetDWorkWidth(S, 364, 1);
34142 ssSetDWorkDataType(S, 364, SS_INT8);
34143 ssSetDWorkUsedAsDState(S, 364, 1);
34144
34145 /* '<S309>/Unit Delay6': DSTATE */
34146 ssSetDWorkName(S, 365, "DWORK365");
34147 ssSetDWorkWidth(S, 365, 1);
34148 ssSetDWorkDataType(S, 365, SS_INT8);
34149 ssSetDWorkUsedAsDState(S, 365, 1);
34150
34151 /* '<S309>/Unit Delay5': DSTATE */
34152 ssSetDWorkName(S, 366, "DWORK366");
34153 ssSetDWorkWidth(S, 366, 1);
34154 ssSetDWorkDataType(S, 366, SS_INT8);
34155 ssSetDWorkUsedAsDState(S, 366, 1);
34156
34157 /* '<S1>/Unit Delay': DSTATE */
34158 ssSetDWorkName(S, 367, "DWORK367");
34159 ssSetDWorkWidth(S, 367, 1);
34160 ssSetDWorkDataType(S, 367, SS_INT8);
34161 ssSetDWorkUsedAsDState(S, 367, 1);
34162
34163 /* '<S430>/Unit Delay6': DSTATE */
34164 ssSetDWorkName(S, 368, "DWORK368");
34165 ssSetDWorkWidth(S, 368, 1);
34166 ssSetDWorkDataType(S, 368, SS_INT8);
34167 ssSetDWorkUsedAsDState(S, 368, 1);
34168
34169 /* '<S336>/Unit Delay2': DSTATE */
34170 ssSetDWorkName(S, 369, "DWORK369");
34171 ssSetDWorkWidth(S, 369, 1);
34172 ssSetDWorkDataType(S, 369, SS_INT8);
34173 ssSetDWorkUsedAsDState(S, 369, 1);
34174
34175 /* '<S430>/Unit Delay5': DSTATE */
34176 ssSetDWorkName(S, 370, "DWORK370");
34177 ssSetDWorkWidth(S, 370, 1);
34178 ssSetDWorkDataType(S, 370, SS_INT8);
34179 ssSetDWorkUsedAsDState(S, 370, 1);
34180
34181 /* '<S682>/Unit Delay': DSTATE */
34182 ssSetDWorkName(S, 371, "DWORK371");
34183 ssSetDWorkWidth(S, 371, 1);
34184 ssSetDWorkDataType(S, 371, SS_INT8);
34185 ssSetDWorkUsedAsDState(S, 371, 1);
34186
34187 /* '<S683>/Unit Delay': DSTATE */
34188 ssSetDWorkName(S, 372, "DWORK372");
34189 ssSetDWorkWidth(S, 372, 1);
34190 ssSetDWorkDataType(S, 372, SS_INT8);
34191 ssSetDWorkUsedAsDState(S, 372, 1);
34192
34193 /* '<S175>/Unit Delay': DSTATE */
34194 ssSetDWorkName(S, 373, "DWORK373");
34195 ssSetDWorkWidth(S, 373, 1);
34196 ssSetDWorkDataType(S, 373, SS_INT8);
34197 ssSetDWorkUsedAsDState(S, 373, 1);
34198
34199 /* '<S174>/Generated S-Function': DWORK2 */
34200 ssSetDWorkName(S, 374, "DWORK374");
34201 ssSetDWorkWidth(S, 374, 1);
34202 ssSetDWorkDataType(S, 374, SS_INT8);
34203 ssSetDWorkUsedAsDState(S, 374, 1);
34204
34205 /* '<S174>/Generated S-Function': DWORK3 */
34206 ssSetDWorkName(S, 375, "DWORK375");
34207 ssSetDWorkWidth(S, 375, 1);
34208 ssSetDWorkDataType(S, 375, SS_INT8);
34209 ssSetDWorkUsedAsDState(S, 375, 1);
34210
34211 /* '<S185>/Generated S-Function': DWORK2 */
34212 ssSetDWorkName(S, 376, "DWORK376");
34213 ssSetDWorkWidth(S, 376, 1);
34214 ssSetDWorkDataType(S, 376, SS_INT8);
34215 ssSetDWorkUsedAsDState(S, 376, 1);
34216
34217 /* '<S185>/Generated S-Function': DWORK3 */
34218 ssSetDWorkName(S, 377, "DWORK377");
34219 ssSetDWorkWidth(S, 377, 1);
34220 ssSetDWorkDataType(S, 377, SS_INT8);
34221 ssSetDWorkUsedAsDState(S, 377, 1);
34222
34223 /* '<S184>/Generated S-Function': DWORK2 */
34224 ssSetDWorkName(S, 378, "DWORK378");
34225 ssSetDWorkWidth(S, 378, 1);
34226 ssSetDWorkDataType(S, 378, SS_INT8);
34227 ssSetDWorkUsedAsDState(S, 378, 1);
34228
34229 /* '<S184>/Generated S-Function': DWORK3 */
34230 ssSetDWorkName(S, 379, "DWORK379");
34231 ssSetDWorkWidth(S, 379, 1);
34232 ssSetDWorkDataType(S, 379, SS_INT8);
34233 ssSetDWorkUsedAsDState(S, 379, 1);
34234
34235 /* '<S183>/Generated S-Function': DWORK2 */
34236 ssSetDWorkName(S, 380, "DWORK380");
34237 ssSetDWorkWidth(S, 380, 1);
34238 ssSetDWorkDataType(S, 380, SS_INT8);
34239 ssSetDWorkUsedAsDState(S, 380, 1);
34240
34241 /* '<S183>/Generated S-Function': DWORK3 */
34242 ssSetDWorkName(S, 381, "DWORK381");
34243 ssSetDWorkWidth(S, 381, 1);
34244 ssSetDWorkDataType(S, 381, SS_INT8);
34245 ssSetDWorkUsedAsDState(S, 381, 1);
34246
34247 /* '<S178>/Generated S-Function': DWORK2 */
34248 ssSetDWorkName(S, 382, "DWORK382");
34249 ssSetDWorkWidth(S, 382, 1);
34250 ssSetDWorkDataType(S, 382, SS_INT8);
34251 ssSetDWorkUsedAsDState(S, 382, 1);
34252
34253 /* '<S178>/Generated S-Function': DWORK3 */
34254 ssSetDWorkName(S, 383, "DWORK383");
34255 ssSetDWorkWidth(S, 383, 1);
34256 ssSetDWorkDataType(S, 383, SS_INT8);
34257 ssSetDWorkUsedAsDState(S, 383, 1);
34258
34259 /* '<S177>/Generated S-Function': DWORK2 */
34260 ssSetDWorkName(S, 384, "DWORK384");
34261 ssSetDWorkWidth(S, 384, 1);
34262 ssSetDWorkDataType(S, 384, SS_INT8);
34263 ssSetDWorkUsedAsDState(S, 384, 1);
34264
34265 /* '<S177>/Generated S-Function': DWORK3 */
34266 ssSetDWorkName(S, 385, "DWORK385");
34267 ssSetDWorkWidth(S, 385, 1);
34268 ssSetDWorkDataType(S, 385, SS_INT8);
34269 ssSetDWorkUsedAsDState(S, 385, 1);
34270
34271 /* '<S176>/Generated S-Function': DWORK2 */
34272 ssSetDWorkName(S, 386, "DWORK386");
34273 ssSetDWorkWidth(S, 386, 1);
34274 ssSetDWorkDataType(S, 386, SS_INT8);
34275 ssSetDWorkUsedAsDState(S, 386, 1);
34276
34277 /* '<S176>/Generated S-Function': DWORK3 */
34278 ssSetDWorkName(S, 387, "DWORK387");
34279 ssSetDWorkWidth(S, 387, 1);
34280 ssSetDWorkDataType(S, 387, SS_INT8);
34281 ssSetDWorkUsedAsDState(S, 387, 1);
34282
34283 /* '<S115>/Unit Delay': DSTATE */
34284 ssSetDWorkName(S, 388, "DWORK388");
34285 ssSetDWorkWidth(S, 388, 1);
34286 ssSetDWorkDataType(S, 388, SS_INT8);
34287 ssSetDWorkUsedAsDState(S, 388, 1);
34288
34289 /* '<S89>/Generated S-Function3': DWORK2 */
34290 ssSetDWorkName(S, 389, "DWORK389");
34291 ssSetDWorkWidth(S, 389, 1);
34292 ssSetDWorkDataType(S, 389, SS_INT8);
34293 ssSetDWorkUsedAsDState(S, 389, 1);
34294
34295 /* '<S89>/Generated S-Function3': DWORK3 */
34296 ssSetDWorkName(S, 390, "DWORK390");
34297 ssSetDWorkWidth(S, 390, 1);
34298 ssSetDWorkDataType(S, 390, SS_INT8);
34299 ssSetDWorkUsedAsDState(S, 390, 1);
34300
34301 /* '<S90>/Generated S-Function2': DWORK2 */
34302 ssSetDWorkName(S, 391, "DWORK391");
34303 ssSetDWorkWidth(S, 391, 1);
34304 ssSetDWorkDataType(S, 391, SS_INT8);
34305 ssSetDWorkUsedAsDState(S, 391, 1);
34306
34307 /* '<S90>/Generated S-Function2': DWORK3 */
34308 ssSetDWorkName(S, 392, "DWORK392");
34309 ssSetDWorkWidth(S, 392, 1);
34310 ssSetDWorkDataType(S, 392, SS_INT8);
34311 ssSetDWorkUsedAsDState(S, 392, 1);
34312
34313 /* '<S87>/Generated S-Function4': DWORK2 */
34314 ssSetDWorkName(S, 393, "DWORK393");
34315 ssSetDWorkWidth(S, 393, 1);
34316 ssSetDWorkDataType(S, 393, SS_INT8);
34317 ssSetDWorkUsedAsDState(S, 393, 1);
34318
34319 /* '<S87>/Generated S-Function4': DWORK3 */
34320 ssSetDWorkName(S, 394, "DWORK394");
34321 ssSetDWorkWidth(S, 394, 1);
34322 ssSetDWorkDataType(S, 394, SS_INT8);
34323 ssSetDWorkUsedAsDState(S, 394, 1);
34324
34325 /* '<S113>/Generated S-Function': DWORK2 */
34326 ssSetDWorkName(S, 395, "DWORK395");
34327 ssSetDWorkWidth(S, 395, 1);
34328 ssSetDWorkDataType(S, 395, SS_INT8);
34329 ssSetDWorkUsedAsDState(S, 395, 1);
34330
34331 /* '<S113>/Generated S-Function': DWORK3 */
34332 ssSetDWorkName(S, 396, "DWORK396");
34333 ssSetDWorkWidth(S, 396, 1);
34334 ssSetDWorkDataType(S, 396, SS_INT8);
34335 ssSetDWorkUsedAsDState(S, 396, 1);
34336
34337 /* '<S118>/Generated S-Function2': DWORK2 */
34338 ssSetDWorkName(S, 397, "DWORK397");
34339 ssSetDWorkWidth(S, 397, 1);
34340 ssSetDWorkDataType(S, 397, SS_INT8);
34341 ssSetDWorkUsedAsDState(S, 397, 1);
34342
34343 /* '<S118>/Generated S-Function2': DWORK3 */
34344 ssSetDWorkName(S, 398, "DWORK398");
34345 ssSetDWorkWidth(S, 398, 1);
34346 ssSetDWorkDataType(S, 398, SS_INT8);
34347 ssSetDWorkUsedAsDState(S, 398, 1);
34348
34349 /* '<S117>/Generated S-Function4': DWORK2 */
34350 ssSetDWorkName(S, 399, "DWORK399");
34351 ssSetDWorkWidth(S, 399, 1);
34352 ssSetDWorkDataType(S, 399, SS_INT8);
34353 ssSetDWorkUsedAsDState(S, 399, 1);
34354
34355 /* '<S117>/Generated S-Function4': DWORK3 */
34356 ssSetDWorkName(S, 400, "DWORK400");
34357 ssSetDWorkWidth(S, 400, 1);
34358 ssSetDWorkDataType(S, 400, SS_INT8);
34359 ssSetDWorkUsedAsDState(S, 400, 1);
34360
34361 /* '<S116>/Generated S-Function4': DWORK2 */
34362 ssSetDWorkName(S, 401, "DWORK401");
34363 ssSetDWorkWidth(S, 401, 1);
34364 ssSetDWorkDataType(S, 401, SS_INT8);
34365 ssSetDWorkUsedAsDState(S, 401, 1);
34366
34367 /* '<S116>/Generated S-Function4': DWORK3 */
34368 ssSetDWorkName(S, 402, "DWORK402");
34369 ssSetDWorkWidth(S, 402, 1);
34370 ssSetDWorkDataType(S, 402, SS_INT8);
34371 ssSetDWorkUsedAsDState(S, 402, 1);
34372
34373 /* '<S100>/Generated S-Function3': DWORK2 */
34374 ssSetDWorkName(S, 403, "DWORK403");
34375 ssSetDWorkWidth(S, 403, 1);
34376 ssSetDWorkDataType(S, 403, SS_INT8);
34377 ssSetDWorkUsedAsDState(S, 403, 1);
34378
34379 /* '<S100>/Generated S-Function3': DWORK3 */
34380 ssSetDWorkName(S, 404, "DWORK404");
34381 ssSetDWorkWidth(S, 404, 1);
34382 ssSetDWorkDataType(S, 404, SS_INT8);
34383 ssSetDWorkUsedAsDState(S, 404, 1);
34384
34385 /* '<S101>/Generated S-Function2': DWORK2 */
34386 ssSetDWorkName(S, 405, "DWORK405");
34387 ssSetDWorkWidth(S, 405, 1);
34388 ssSetDWorkDataType(S, 405, SS_INT8);
34389 ssSetDWorkUsedAsDState(S, 405, 1);
34390
34391 /* '<S101>/Generated S-Function2': DWORK3 */
34392 ssSetDWorkName(S, 406, "DWORK406");
34393 ssSetDWorkWidth(S, 406, 1);
34394 ssSetDWorkDataType(S, 406, SS_INT8);
34395 ssSetDWorkUsedAsDState(S, 406, 1);
34396
34397 /* '<S98>/Generated S-Function4': DWORK2 */
34398 ssSetDWorkName(S, 407, "DWORK407");
34399 ssSetDWorkWidth(S, 407, 1);
34400 ssSetDWorkDataType(S, 407, SS_INT8);
34401 ssSetDWorkUsedAsDState(S, 407, 1);
34402
34403 /* '<S98>/Generated S-Function4': DWORK3 */
34404 ssSetDWorkName(S, 408, "DWORK408");
34405 ssSetDWorkWidth(S, 408, 1);
34406 ssSetDWorkDataType(S, 408, SS_INT8);
34407 ssSetDWorkUsedAsDState(S, 408, 1);
34408
34409 /* '<S107>/Generated S-Function3': DWORK2 */
34410 ssSetDWorkName(S, 409, "DWORK409");
34411 ssSetDWorkWidth(S, 409, 1);
34412 ssSetDWorkDataType(S, 409, SS_INT8);
34413 ssSetDWorkUsedAsDState(S, 409, 1);
34414
34415 /* '<S107>/Generated S-Function3': DWORK3 */
34416 ssSetDWorkName(S, 410, "DWORK410");
34417 ssSetDWorkWidth(S, 410, 1);
34418 ssSetDWorkDataType(S, 410, SS_INT8);
34419 ssSetDWorkUsedAsDState(S, 410, 1);
34420
34421 /* '<S108>/Generated S-Function2': DWORK2 */
34422 ssSetDWorkName(S, 411, "DWORK411");
34423 ssSetDWorkWidth(S, 411, 1);
34424 ssSetDWorkDataType(S, 411, SS_INT8);
34425 ssSetDWorkUsedAsDState(S, 411, 1);
34426
34427 /* '<S108>/Generated S-Function2': DWORK3 */
34428 ssSetDWorkName(S, 412, "DWORK412");
34429 ssSetDWorkWidth(S, 412, 1);
34430 ssSetDWorkDataType(S, 412, SS_INT8);
34431 ssSetDWorkUsedAsDState(S, 412, 1);
34432
34433 /* '<S106>/Generated S-Function4': DWORK2 */
34434 ssSetDWorkName(S, 413, "DWORK413");
34435 ssSetDWorkWidth(S, 413, 1);
34436 ssSetDWorkDataType(S, 413, SS_INT8);
34437 ssSetDWorkUsedAsDState(S, 413, 1);
34438
34439 /* '<S106>/Generated S-Function4': DWORK3 */
34440 ssSetDWorkName(S, 414, "DWORK414");
34441 ssSetDWorkWidth(S, 414, 1);
34442 ssSetDWorkDataType(S, 414, SS_INT8);
34443 ssSetDWorkUsedAsDState(S, 414, 1);
34444
34445 /* '<S56>/Unit Delay': DSTATE */
34446 ssSetDWorkName(S, 415, "DWORK415");
34447 ssSetDWorkWidth(S, 415, 1);
34448 ssSetDWorkDataType(S, 415, SS_INT8);
34449 ssSetDWorkUsedAsDState(S, 415, 1);
34450
34451 /* '<S138>/Unit Delay': DSTATE */
34452 ssSetDWorkName(S, 416, "DWORK416");
34453 ssSetDWorkWidth(S, 416, 1);
34454 ssSetDWorkDataType(S, 416, SS_INT8);
34455 ssSetDWorkUsedAsDState(S, 416, 1);
34456
34457 /* '<S130>/Generated S-Function4': DWORK2 */
34458 ssSetDWorkName(S, 417, "DWORK417");
34459 ssSetDWorkWidth(S, 417, 1);
34460 ssSetDWorkDataType(S, 417, SS_INT8);
34461 ssSetDWorkUsedAsDState(S, 417, 1);
34462
34463 /* '<S130>/Generated S-Function4': DWORK3 */
34464 ssSetDWorkName(S, 418, "DWORK418");
34465 ssSetDWorkWidth(S, 418, 1);
34466 ssSetDWorkDataType(S, 418, SS_INT8);
34467 ssSetDWorkUsedAsDState(S, 418, 1);
34468
34469 /* '<S132>/Generated S-Function3': DWORK2 */
34470 ssSetDWorkName(S, 419, "DWORK419");
34471 ssSetDWorkWidth(S, 419, 1);
34472 ssSetDWorkDataType(S, 419, SS_INT8);
34473 ssSetDWorkUsedAsDState(S, 419, 1);
34474
34475 /* '<S132>/Generated S-Function3': DWORK3 */
34476 ssSetDWorkName(S, 420, "DWORK420");
34477 ssSetDWorkWidth(S, 420, 1);
34478 ssSetDWorkDataType(S, 420, SS_INT8);
34479 ssSetDWorkUsedAsDState(S, 420, 1);
34480
34481 /* '<S133>/Generated S-Function2': DWORK2 */
34482 ssSetDWorkName(S, 421, "DWORK421");
34483 ssSetDWorkWidth(S, 421, 1);
34484 ssSetDWorkDataType(S, 421, SS_INT8);
34485 ssSetDWorkUsedAsDState(S, 421, 1);
34486
34487 /* '<S133>/Generated S-Function2': DWORK3 */
34488 ssSetDWorkName(S, 422, "DWORK422");
34489 ssSetDWorkWidth(S, 422, 1);
34490 ssSetDWorkDataType(S, 422, SS_INT8);
34491 ssSetDWorkUsedAsDState(S, 422, 1);
34492
34493 /* '<S131>/Generated S-Function4': DWORK2 */
34494 ssSetDWorkName(S, 423, "DWORK423");
34495 ssSetDWorkWidth(S, 423, 1);
34496 ssSetDWorkDataType(S, 423, SS_INT8);
34497 ssSetDWorkUsedAsDState(S, 423, 1);
34498
34499 /* '<S131>/Generated S-Function4': DWORK3 */
34500 ssSetDWorkName(S, 424, "DWORK424");
34501 ssSetDWorkWidth(S, 424, 1);
34502 ssSetDWorkDataType(S, 424, SS_INT8);
34503 ssSetDWorkUsedAsDState(S, 424, 1);
34504
34505 /* '<S152>/Generated S-Function2': DWORK2 */
34506 ssSetDWorkName(S, 425, "DWORK425");
34507 ssSetDWorkWidth(S, 425, 1);
34508 ssSetDWorkDataType(S, 425, SS_INT8);
34509 ssSetDWorkUsedAsDState(S, 425, 1);
34510
34511 /* '<S152>/Generated S-Function2': DWORK3 */
34512 ssSetDWorkName(S, 426, "DWORK426");
34513 ssSetDWorkWidth(S, 426, 1);
34514 ssSetDWorkDataType(S, 426, SS_INT8);
34515 ssSetDWorkUsedAsDState(S, 426, 1);
34516
34517 /* '<S150>/Generated S-Function3': DWORK2 */
34518 ssSetDWorkName(S, 427, "DWORK427");
34519 ssSetDWorkWidth(S, 427, 1);
34520 ssSetDWorkDataType(S, 427, SS_INT8);
34521 ssSetDWorkUsedAsDState(S, 427, 1);
34522
34523 /* '<S150>/Generated S-Function3': DWORK3 */
34524 ssSetDWorkName(S, 428, "DWORK428");
34525 ssSetDWorkWidth(S, 428, 1);
34526 ssSetDWorkDataType(S, 428, SS_INT8);
34527 ssSetDWorkUsedAsDState(S, 428, 1);
34528
34529 /* '<S151>/Generated S-Function2': DWORK2 */
34530 ssSetDWorkName(S, 429, "DWORK429");
34531 ssSetDWorkWidth(S, 429, 1);
34532 ssSetDWorkDataType(S, 429, SS_INT8);
34533 ssSetDWorkUsedAsDState(S, 429, 1);
34534
34535 /* '<S151>/Generated S-Function2': DWORK3 */
34536 ssSetDWorkName(S, 430, "DWORK430");
34537 ssSetDWorkWidth(S, 430, 1);
34538 ssSetDWorkDataType(S, 430, SS_INT8);
34539 ssSetDWorkUsedAsDState(S, 430, 1);
34540
34541 /* '<S149>/Generated S-Function4': DWORK2 */
34542 ssSetDWorkName(S, 431, "DWORK431");
34543 ssSetDWorkWidth(S, 431, 1);
34544 ssSetDWorkDataType(S, 431, SS_INT8);
34545 ssSetDWorkUsedAsDState(S, 431, 1);
34546
34547 /* '<S149>/Generated S-Function4': DWORK3 */
34548 ssSetDWorkName(S, 432, "DWORK432");
34549 ssSetDWorkWidth(S, 432, 1);
34550 ssSetDWorkDataType(S, 432, SS_INT8);
34551 ssSetDWorkUsedAsDState(S, 432, 1);
34552
34553 /* '<S142>/Generated S-Function2': DWORK2 */
34554 ssSetDWorkName(S, 433, "DWORK433");
34555 ssSetDWorkWidth(S, 433, 1);
34556 ssSetDWorkDataType(S, 433, SS_INT8);
34557 ssSetDWorkUsedAsDState(S, 433, 1);
34558
34559 /* '<S142>/Generated S-Function2': DWORK3 */
34560 ssSetDWorkName(S, 434, "DWORK434");
34561 ssSetDWorkWidth(S, 434, 1);
34562 ssSetDWorkDataType(S, 434, SS_INT8);
34563 ssSetDWorkUsedAsDState(S, 434, 1);
34564
34565 /* '<S140>/Generated S-Function3': DWORK2 */
34566 ssSetDWorkName(S, 435, "DWORK435");
34567 ssSetDWorkWidth(S, 435, 1);
34568 ssSetDWorkDataType(S, 435, SS_INT8);
34569 ssSetDWorkUsedAsDState(S, 435, 1);
34570
34571 /* '<S140>/Generated S-Function3': DWORK3 */
34572 ssSetDWorkName(S, 436, "DWORK436");
34573 ssSetDWorkWidth(S, 436, 1);
34574 ssSetDWorkDataType(S, 436, SS_INT8);
34575 ssSetDWorkUsedAsDState(S, 436, 1);
34576
34577 /* '<S141>/Generated S-Function2': DWORK2 */
34578 ssSetDWorkName(S, 437, "DWORK437");
34579 ssSetDWorkWidth(S, 437, 1);
34580 ssSetDWorkDataType(S, 437, SS_INT8);
34581 ssSetDWorkUsedAsDState(S, 437, 1);
34582
34583 /* '<S141>/Generated S-Function2': DWORK3 */
34584 ssSetDWorkName(S, 438, "DWORK438");
34585 ssSetDWorkWidth(S, 438, 1);
34586 ssSetDWorkDataType(S, 438, SS_INT8);
34587 ssSetDWorkUsedAsDState(S, 438, 1);
34588
34589 /* '<S139>/Generated S-Function4': DWORK2 */
34590 ssSetDWorkName(S, 439, "DWORK439");
34591 ssSetDWorkWidth(S, 439, 1);
34592 ssSetDWorkDataType(S, 439, SS_INT8);
34593 ssSetDWorkUsedAsDState(S, 439, 1);
34594
34595 /* '<S139>/Generated S-Function4': DWORK3 */
34596 ssSetDWorkName(S, 440, "DWORK440");
34597 ssSetDWorkWidth(S, 440, 1);
34598 ssSetDWorkDataType(S, 440, SS_INT8);
34599 ssSetDWorkUsedAsDState(S, 440, 1);
34600
34601 /* '<S160>/Generated S-Function3': DWORK2 */
34602 ssSetDWorkName(S, 441, "DWORK441");
34603 ssSetDWorkWidth(S, 441, 1);
34604 ssSetDWorkDataType(S, 441, SS_INT8);
34605 ssSetDWorkUsedAsDState(S, 441, 1);
34606
34607 /* '<S160>/Generated S-Function3': DWORK3 */
34608 ssSetDWorkName(S, 442, "DWORK442");
34609 ssSetDWorkWidth(S, 442, 1);
34610 ssSetDWorkDataType(S, 442, SS_INT8);
34611 ssSetDWorkUsedAsDState(S, 442, 1);
34612
34613 /* '<S159>/Generated S-Function4': DWORK2 */
34614 ssSetDWorkName(S, 443, "DWORK443");
34615 ssSetDWorkWidth(S, 443, 1);
34616 ssSetDWorkDataType(S, 443, SS_INT8);
34617 ssSetDWorkUsedAsDState(S, 443, 1);
34618
34619 /* '<S159>/Generated S-Function4': DWORK3 */
34620 ssSetDWorkName(S, 444, "DWORK444");
34621 ssSetDWorkWidth(S, 444, 1);
34622 ssSetDWorkDataType(S, 444, SS_INT8);
34623 ssSetDWorkUsedAsDState(S, 444, 1);
34624
34625 /* '<S195>/Unit Delay': DSTATE */
34626 ssSetDWorkName(S, 445, "DWORK445");
34627 ssSetDWorkWidth(S, 445, 1);
34628 ssSetDWorkDataType(S, 445, SS_INT8);
34629 ssSetDWorkUsedAsDState(S, 445, 1);
34630
34631 /* '<S190>/Generated S-Function4': DWORK2 */
34632 ssSetDWorkName(S, 446, "DWORK446");
34633 ssSetDWorkWidth(S, 446, 1);
34634 ssSetDWorkDataType(S, 446, SS_INT8);
34635 ssSetDWorkUsedAsDState(S, 446, 1);
34636
34637 /* '<S190>/Generated S-Function4': DWORK3 */
34638 ssSetDWorkName(S, 447, "DWORK447");
34639 ssSetDWorkWidth(S, 447, 1);
34640 ssSetDWorkDataType(S, 447, SS_INT8);
34641 ssSetDWorkUsedAsDState(S, 447, 1);
34642
34643 /* '<S191>/Generated S-Function4': DWORK2 */
34644 ssSetDWorkName(S, 448, "DWORK448");
34645 ssSetDWorkWidth(S, 448, 1);
34646 ssSetDWorkDataType(S, 448, SS_INT8);
34647 ssSetDWorkUsedAsDState(S, 448, 1);
34648
34649 /* '<S191>/Generated S-Function4': DWORK3 */
34650 ssSetDWorkName(S, 449, "DWORK449");
34651 ssSetDWorkWidth(S, 449, 1);
34652 ssSetDWorkDataType(S, 449, SS_INT8);
34653 ssSetDWorkUsedAsDState(S, 449, 1);
34654
34655 /* '<S197>/Generated S-Function2': DWORK2 */
34656 ssSetDWorkName(S, 450, "DWORK450");
34657 ssSetDWorkWidth(S, 450, 1);
34658 ssSetDWorkDataType(S, 450, SS_INT8);
34659 ssSetDWorkUsedAsDState(S, 450, 1);
34660
34661 /* '<S197>/Generated S-Function2': DWORK3 */
34662 ssSetDWorkName(S, 451, "DWORK451");
34663 ssSetDWorkWidth(S, 451, 1);
34664 ssSetDWorkDataType(S, 451, SS_INT8);
34665 ssSetDWorkUsedAsDState(S, 451, 1);
34666
34667 /* '<S196>/Generated S-Function4': DWORK2 */
34668 ssSetDWorkName(S, 452, "DWORK452");
34669 ssSetDWorkWidth(S, 452, 1);
34670 ssSetDWorkDataType(S, 452, SS_INT8);
34671 ssSetDWorkUsedAsDState(S, 452, 1);
34672
34673 /* '<S196>/Generated S-Function4': DWORK3 */
34674 ssSetDWorkName(S, 453, "DWORK453");
34675 ssSetDWorkWidth(S, 453, 1);
34676 ssSetDWorkDataType(S, 453, SS_INT8);
34677 ssSetDWorkUsedAsDState(S, 453, 1);
34678
34679 /* '<S201>/Generated S-Function2': DWORK2 */
34680 ssSetDWorkName(S, 454, "DWORK454");
34681 ssSetDWorkWidth(S, 454, 1);
34682 ssSetDWorkDataType(S, 454, SS_INT8);
34683 ssSetDWorkUsedAsDState(S, 454, 1);
34684
34685 /* '<S201>/Generated S-Function2': DWORK3 */
34686 ssSetDWorkName(S, 455, "DWORK455");
34687 ssSetDWorkWidth(S, 455, 1);
34688 ssSetDWorkDataType(S, 455, SS_INT8);
34689 ssSetDWorkUsedAsDState(S, 455, 1);
34690
34691 /* '<S200>/Generated S-Function4': DWORK2 */
34692 ssSetDWorkName(S, 456, "DWORK456");
34693 ssSetDWorkWidth(S, 456, 1);
34694 ssSetDWorkDataType(S, 456, SS_INT8);
34695 ssSetDWorkUsedAsDState(S, 456, 1);
34696
34697 /* '<S200>/Generated S-Function4': DWORK3 */
34698 ssSetDWorkName(S, 457, "DWORK457");
34699 ssSetDWorkWidth(S, 457, 1);
34700 ssSetDWorkDataType(S, 457, SS_INT8);
34701 ssSetDWorkUsedAsDState(S, 457, 1);
34702
34703 /* '<S73>/Unit Delay': DSTATE */
34704 ssSetDWorkName(S, 458, "DWORK458");
34705 ssSetDWorkWidth(S, 458, 1);
34706 ssSetDWorkDataType(S, 458, SS_INT8);
34707 ssSetDWorkUsedAsDState(S, 458, 1);
34708
34709 /* '<S69>/Generated S-Function4': DWORK2 */
34710 ssSetDWorkName(S, 459, "DWORK459");
34711 ssSetDWorkWidth(S, 459, 1);
34712 ssSetDWorkDataType(S, 459, SS_INT8);
34713 ssSetDWorkUsedAsDState(S, 459, 1);
34714
34715 /* '<S69>/Generated S-Function4': DWORK3 */
34716 ssSetDWorkName(S, 460, "DWORK460");
34717 ssSetDWorkWidth(S, 460, 1);
34718 ssSetDWorkDataType(S, 460, SS_INT8);
34719 ssSetDWorkUsedAsDState(S, 460, 1);
34720
34721 /* '<S70>/Generated S-Function4': DWORK2 */
34722 ssSetDWorkName(S, 461, "DWORK461");
34723 ssSetDWorkWidth(S, 461, 1);
34724 ssSetDWorkDataType(S, 461, SS_INT8);
34725 ssSetDWorkUsedAsDState(S, 461, 1);
34726
34727 /* '<S70>/Generated S-Function4': DWORK3 */
34728 ssSetDWorkName(S, 462, "DWORK462");
34729 ssSetDWorkWidth(S, 462, 1);
34730 ssSetDWorkDataType(S, 462, SS_INT8);
34731 ssSetDWorkUsedAsDState(S, 462, 1);
34732
34733 /* '<S75>/Generated S-Function2': DWORK2 */
34734 ssSetDWorkName(S, 463, "DWORK463");
34735 ssSetDWorkWidth(S, 463, 1);
34736 ssSetDWorkDataType(S, 463, SS_INT8);
34737 ssSetDWorkUsedAsDState(S, 463, 1);
34738
34739 /* '<S75>/Generated S-Function2': DWORK3 */
34740 ssSetDWorkName(S, 464, "DWORK464");
34741 ssSetDWorkWidth(S, 464, 1);
34742 ssSetDWorkDataType(S, 464, SS_INT8);
34743 ssSetDWorkUsedAsDState(S, 464, 1);
34744
34745 /* '<S74>/Generated S-Function4': DWORK2 */
34746 ssSetDWorkName(S, 465, "DWORK465");
34747 ssSetDWorkWidth(S, 465, 1);
34748 ssSetDWorkDataType(S, 465, SS_INT8);
34749 ssSetDWorkUsedAsDState(S, 465, 1);
34750
34751 /* '<S74>/Generated S-Function4': DWORK3 */
34752 ssSetDWorkName(S, 466, "DWORK466");
34753 ssSetDWorkWidth(S, 466, 1);
34754 ssSetDWorkDataType(S, 466, SS_INT8);
34755 ssSetDWorkUsedAsDState(S, 466, 1);
34756
34757 /* '<S208>/Generated S-Function': DWORK1 */
34758 ssSetDWorkName(S, 467, "DWORK467");
34759 ssSetDWorkWidth(S, 467, 1);
34760 ssSetDWorkDataType(S, 467, SS_INT8);
34761 ssSetDWorkUsedAsDState(S, 467, 1);
34762
34763 /* '<S208>/Generated S-Function': DWORK2 */
34764 ssSetDWorkName(S, 468, "DWORK468");
34765 ssSetDWorkWidth(S, 468, 1);
34766 ssSetDWorkDataType(S, 468, SS_INT8);
34767 ssSetDWorkUsedAsDState(S, 468, 1);
34768
34769 /* '<S241>/Generated S-Function4': DWORK2 */
34770 ssSetDWorkName(S, 469, "DWORK469");
34771 ssSetDWorkWidth(S, 469, 1);
34772 ssSetDWorkDataType(S, 469, SS_INT8);
34773 ssSetDWorkUsedAsDState(S, 469, 1);
34774
34775 /* '<S241>/Generated S-Function4': DWORK3 */
34776 ssSetDWorkName(S, 470, "DWORK470");
34777 ssSetDWorkWidth(S, 470, 1);
34778 ssSetDWorkDataType(S, 470, SS_INT8);
34779 ssSetDWorkUsedAsDState(S, 470, 1);
34780
34781 /* '<S239>/Generated S-Function4': DWORK2 */
34782 ssSetDWorkName(S, 471, "DWORK471");
34783 ssSetDWorkWidth(S, 471, 1);
34784 ssSetDWorkDataType(S, 471, SS_INT8);
34785 ssSetDWorkUsedAsDState(S, 471, 1);
34786
34787 /* '<S239>/Generated S-Function4': DWORK3 */
34788 ssSetDWorkName(S, 472, "DWORK472");
34789 ssSetDWorkWidth(S, 472, 1);
34790 ssSetDWorkDataType(S, 472, SS_INT8);
34791 ssSetDWorkUsedAsDState(S, 472, 1);
34792
34793 /* '<S240>/Generated S-Function4': DWORK2 */
34794 ssSetDWorkName(S, 473, "DWORK473");
34795 ssSetDWorkWidth(S, 473, 1);
34796 ssSetDWorkDataType(S, 473, SS_INT8);
34797 ssSetDWorkUsedAsDState(S, 473, 1);
34798
34799 /* '<S240>/Generated S-Function4': DWORK3 */
34800 ssSetDWorkName(S, 474, "DWORK474");
34801 ssSetDWorkWidth(S, 474, 1);
34802 ssSetDWorkDataType(S, 474, SS_INT8);
34803 ssSetDWorkUsedAsDState(S, 474, 1);
34804
34805 /* '<S238>/Generated S-Function8': DWORK2 */
34806 ssSetDWorkName(S, 475, "DWORK475");
34807 ssSetDWorkWidth(S, 475, 1);
34808 ssSetDWorkDataType(S, 475, SS_INT8);
34809 ssSetDWorkUsedAsDState(S, 475, 1);
34810
34811 /* '<S238>/Generated S-Function8': DWORK3 */
34812 ssSetDWorkName(S, 476, "DWORK476");
34813 ssSetDWorkWidth(S, 476, 1);
34814 ssSetDWorkDataType(S, 476, SS_INT8);
34815 ssSetDWorkUsedAsDState(S, 476, 1);
34816
34817 /* '<S223>/Generated S-Function7': DWORK1 */
34818 ssSetDWorkName(S, 477, "DWORK477");
34819 ssSetDWorkWidth(S, 477, 1);
34820 ssSetDWorkDataType(S, 477, SS_INT8);
34821 ssSetDWorkUsedAsDState(S, 477, 1);
34822
34823 /* '<S223>/Generated S-Function7': DWORK2 */
34824 ssSetDWorkName(S, 478, "DWORK478");
34825 ssSetDWorkWidth(S, 478, 1);
34826 ssSetDWorkDataType(S, 478, SS_INT8);
34827 ssSetDWorkUsedAsDState(S, 478, 1);
34828
34829 /* '<S224>/Generated S-Function7': DWORK1 */
34830 ssSetDWorkName(S, 479, "DWORK479");
34831 ssSetDWorkWidth(S, 479, 1);
34832 ssSetDWorkDataType(S, 479, SS_INT8);
34833 ssSetDWorkUsedAsDState(S, 479, 1);
34834
34835 /* '<S224>/Generated S-Function7': DWORK2 */
34836 ssSetDWorkName(S, 480, "DWORK480");
34837 ssSetDWorkWidth(S, 480, 1);
34838 ssSetDWorkDataType(S, 480, SS_INT8);
34839 ssSetDWorkUsedAsDState(S, 480, 1);
34840
34841 /* '<S252>/Generated S-Function8': DWORK2 */
34842 ssSetDWorkName(S, 481, "DWORK481");
34843 ssSetDWorkWidth(S, 481, 1);
34844 ssSetDWorkDataType(S, 481, SS_INT8);
34845 ssSetDWorkUsedAsDState(S, 481, 1);
34846
34847 /* '<S252>/Generated S-Function8': DWORK3 */
34848 ssSetDWorkName(S, 482, "DWORK482");
34849 ssSetDWorkWidth(S, 482, 1);
34850 ssSetDWorkDataType(S, 482, SS_INT8);
34851 ssSetDWorkUsedAsDState(S, 482, 1);
34852
34853 /* '<S253>/Generated S-Function4': DWORK2 */
34854 ssSetDWorkName(S, 483, "DWORK483");
34855 ssSetDWorkWidth(S, 483, 1);
34856 ssSetDWorkDataType(S, 483, SS_INT8);
34857 ssSetDWorkUsedAsDState(S, 483, 1);
34858
34859 /* '<S253>/Generated S-Function4': DWORK3 */
34860 ssSetDWorkName(S, 484, "DWORK484");
34861 ssSetDWorkWidth(S, 484, 1);
34862 ssSetDWorkDataType(S, 484, SS_INT8);
34863 ssSetDWorkUsedAsDState(S, 484, 1);
34864
34865 /* '<S254>/Generated S-Function4': DWORK2 */
34866 ssSetDWorkName(S, 485, "DWORK485");
34867 ssSetDWorkWidth(S, 485, 1);
34868 ssSetDWorkDataType(S, 485, SS_INT8);
34869 ssSetDWorkUsedAsDState(S, 485, 1);
34870
34871 /* '<S254>/Generated S-Function4': DWORK3 */
34872 ssSetDWorkName(S, 486, "DWORK486");
34873 ssSetDWorkWidth(S, 486, 1);
34874 ssSetDWorkDataType(S, 486, SS_INT8);
34875 ssSetDWorkUsedAsDState(S, 486, 1);
34876
34877 /* '<S266>/Generated S-Function': DWORK2 */
34878 ssSetDWorkName(S, 487, "DWORK487");
34879 ssSetDWorkWidth(S, 487, 1);
34880 ssSetDWorkDataType(S, 487, SS_INT8);
34881 ssSetDWorkUsedAsDState(S, 487, 1);
34882
34883 /* '<S266>/Generated S-Function': DWORK3 */
34884 ssSetDWorkName(S, 488, "DWORK488");
34885 ssSetDWorkWidth(S, 488, 1);
34886 ssSetDWorkDataType(S, 488, SS_INT8);
34887 ssSetDWorkUsedAsDState(S, 488, 1);
34888
34889 /* '<S265>/Generated S-Function8': DWORK2 */
34890 ssSetDWorkName(S, 489, "DWORK489");
34891 ssSetDWorkWidth(S, 489, 1);
34892 ssSetDWorkDataType(S, 489, SS_INT8);
34893 ssSetDWorkUsedAsDState(S, 489, 1);
34894
34895 /* '<S265>/Generated S-Function8': DWORK3 */
34896 ssSetDWorkName(S, 490, "DWORK490");
34897 ssSetDWorkWidth(S, 490, 1);
34898 ssSetDWorkDataType(S, 490, SS_INT8);
34899 ssSetDWorkUsedAsDState(S, 490, 1);
34900
34901 /* '<S273>/Unit Delay': DSTATE */
34902 ssSetDWorkName(S, 491, "DWORK491");
34903 ssSetDWorkWidth(S, 491, 1);
34904 ssSetDWorkDataType(S, 491, SS_INT8);
34905 ssSetDWorkUsedAsDState(S, 491, 1);
34906
34907 /* '<S257>/Generated S-Function3': DWORK2 */
34908 ssSetDWorkName(S, 492, "DWORK492");
34909 ssSetDWorkWidth(S, 492, 1);
34910 ssSetDWorkDataType(S, 492, SS_INT8);
34911 ssSetDWorkUsedAsDState(S, 492, 1);
34912
34913 /* '<S257>/Generated S-Function3': DWORK3 */
34914 ssSetDWorkName(S, 493, "DWORK493");
34915 ssSetDWorkWidth(S, 493, 1);
34916 ssSetDWorkDataType(S, 493, SS_INT8);
34917 ssSetDWorkUsedAsDState(S, 493, 1);
34918
34919 /* '<S249>/Generated S-Function7': DWORK1 */
34920 ssSetDWorkName(S, 494, "DWORK494");
34921 ssSetDWorkWidth(S, 494, 1);
34922 ssSetDWorkDataType(S, 494, SS_INT8);
34923 ssSetDWorkUsedAsDState(S, 494, 1);
34924
34925 /* '<S249>/Generated S-Function7': DWORK2 */
34926 ssSetDWorkName(S, 495, "DWORK495");
34927 ssSetDWorkWidth(S, 495, 1);
34928 ssSetDWorkDataType(S, 495, SS_INT8);
34929 ssSetDWorkUsedAsDState(S, 495, 1);
34930
34931 /* '<S274>/Generated S-Function7': DWORK1 */
34932 ssSetDWorkName(S, 496, "DWORK496");
34933 ssSetDWorkWidth(S, 496, 1);
34934 ssSetDWorkDataType(S, 496, SS_INT8);
34935 ssSetDWorkUsedAsDState(S, 496, 1);
34936
34937 /* '<S274>/Generated S-Function7': DWORK2 */
34938 ssSetDWorkName(S, 497, "DWORK497");
34939 ssSetDWorkWidth(S, 497, 1);
34940 ssSetDWorkDataType(S, 497, SS_INT8);
34941 ssSetDWorkUsedAsDState(S, 497, 1);
34942
34943 /* '<S215>/Generated S-Function4': DWORK2 */
34944 ssSetDWorkName(S, 498, "DWORK498");
34945 ssSetDWorkWidth(S, 498, 1);
34946 ssSetDWorkDataType(S, 498, SS_INT8);
34947 ssSetDWorkUsedAsDState(S, 498, 1);
34948
34949 /* '<S215>/Generated S-Function4': DWORK3 */
34950 ssSetDWorkName(S, 499, "DWORK499");
34951 ssSetDWorkWidth(S, 499, 1);
34952 ssSetDWorkDataType(S, 499, SS_INT8);
34953 ssSetDWorkUsedAsDState(S, 499, 1);
34954
34955 /* '<S216>/Generated S-Function7': DWORK1 */
34956 ssSetDWorkName(S, 500, "DWORK500");
34957 ssSetDWorkWidth(S, 500, 1);
34958 ssSetDWorkDataType(S, 500, SS_INT8);
34959 ssSetDWorkUsedAsDState(S, 500, 1);
34960
34961 /* '<S216>/Generated S-Function7': DWORK2 */
34962 ssSetDWorkName(S, 501, "DWORK501");
34963 ssSetDWorkWidth(S, 501, 1);
34964 ssSetDWorkDataType(S, 501, SS_INT8);
34965 ssSetDWorkUsedAsDState(S, 501, 1);
34966
34967 /* '<S310>/Delay': DSTATE */
34968 ssSetDWorkName(S, 502, "DWORK502");
34969 ssSetDWorkWidth(S, 502, 1);
34970 ssSetDWorkDataType(S, 502, SS_BOOLEAN);
34971 ssSetDWorkUsedAsDState(S, 502, 1);
34972
34973 /* '<S431>/Delay': DSTATE */
34974 ssSetDWorkName(S, 503, "DWORK503");
34975 ssSetDWorkWidth(S, 503, 1);
34976 ssSetDWorkDataType(S, 503, SS_BOOLEAN);
34977 ssSetDWorkUsedAsDState(S, 503, 1);
34978
34979 /* '<S9>/Memory2': PreviousInput */
34980 ssSetDWorkName(S, 504, "DWORK504");
34981 ssSetDWorkWidth(S, 504, 1);
34982 ssSetDWorkDataType(S, 504, SS_INT8);
34983
34984 /* '<S31>/Switch Case': ActiveSubsystem */
34985 ssSetDWorkName(S, 505, "DWORK505");
34986 ssSetDWorkWidth(S, 505, 1);
34987 ssSetDWorkDataType(S, 505, SS_INT8);
34988
34989 /* '<S847>/If': ActiveSubsystem */
34990 ssSetDWorkName(S, 506, "DWORK506");
34991 ssSetDWorkWidth(S, 506, 1);
34992 ssSetDWorkDataType(S, 506, SS_INT8);
34993
34994 /* '<S846>/If': ActiveSubsystem */
34995 ssSetDWorkName(S, 507, "DWORK507");
34996 ssSetDWorkWidth(S, 507, 1);
34997 ssSetDWorkDataType(S, 507, SS_INT8);
34998
34999 /* '<S848>/If': ActiveSubsystem */
35000 ssSetDWorkName(S, 508, "DWORK508");
35001 ssSetDWorkWidth(S, 508, 1);
35002 ssSetDWorkDataType(S, 508, SS_INT8);
35003
35004 /* '<S902>/Find Maximum Diagonal Value': ActiveSubsystem */
35005 ssSetDWorkName(S, 509, "DWORK509");
35006 ssSetDWorkWidth(S, 509, 1);
35007 ssSetDWorkDataType(S, 509, SS_INT8);
35008
35009 /* '<S877>/Find Maximum Diagonal Value': ActiveSubsystem */
35010 ssSetDWorkName(S, 510, "DWORK510");
35011 ssSetDWorkWidth(S, 510, 1);
35012 ssSetDWorkDataType(S, 510, SS_INT8);
35013
35014 /* '<S316>/Landing2': DWORK25 */
35015 ssSetDWorkName(S, 511, "DWORK511");
35016 ssSetDWorkWidth(S, 511, 1);
35017 ssSetDWorkDataType(S, 511, SS_INT8);
35018
35019 /* '<S316>/Landing2': DWORK30 */
35020 ssSetDWorkName(S, 512, "DWORK512");
35021 ssSetDWorkWidth(S, 512, 1);
35022 ssSetDWorkDataType(S, 512, SS_INT8);
35023
35024 /* '<S313>/Approach2': DWORK15 */
35025 ssSetDWorkName(S, 513, "DWORK513");
35026 ssSetDWorkWidth(S, 513, 1);
35027 ssSetDWorkDataType(S, 513, SS_INT8);
35028
35029 /* '<S31>/LogicTrans': DWORK1 */
35030 ssSetDWorkName(S, 514, "DWORK514");
35031 ssSetDWorkWidth(S, 514, 1);
35032 ssSetDWorkDataType(S, 514, SS_INT8);
35033
35034 /* '<S299>/AirLineUpdateSucModel': DWORK4 */
35035 ssSetDWorkName(S, 515, "DWORK515");
35036 ssSetDWorkWidth(S, 515, 1);
35037 ssSetDWorkDataType(S, 515, SS_INT8);
35038
35039 /* '<S293>/CompTP': DWORK3 */
35040 ssSetDWorkName(S, 516, "DWORK516");
35041 ssSetDWorkWidth(S, 516, 1);
35042 ssSetDWorkDataType(S, 516, SS_INT8);
35043
35044 /* '<S81>/Generated S-Function5': DWORK5 */
35045 ssSetDWorkName(S, 517, "DWORK517");
35046 ssSetDWorkWidth(S, 517, 1);
35047 ssSetDWorkDataType(S, 517, SS_INT8);
35048
35049 /* '<S81>/Generated S-Function5': DWORK6 */
35050 ssSetDWorkName(S, 518, "DWORK518");
35051 ssSetDWorkWidth(S, 518, 1);
35052 ssSetDWorkDataType(S, 518, SS_INT8);
35053
35054 /* '<S123>/Generated S-Function5': DWORK5 */
35055 ssSetDWorkName(S, 519, "DWORK519");
35056 ssSetDWorkWidth(S, 519, 1);
35057 ssSetDWorkDataType(S, 519, SS_INT8);
35058
35059 /* '<S123>/Generated S-Function5': DWORK6 */
35060 ssSetDWorkName(S, 520, "DWORK520");
35061 ssSetDWorkWidth(S, 520, 1);
35062 ssSetDWorkDataType(S, 520, SS_INT8);
35063
35064 /* '<S12>/LLACalibration2': DWORK1 */
35065 ssSetDWorkName(S, 521, "DWORK521");
35066 ssSetDWorkWidth(S, 521, 1);
35067 ssSetDWorkDataType(S, 521, SS_UINT8);
35068
35069 /* '<S12>/LLACalibration2': DWORK2 */
35070 ssSetDWorkName(S, 522, "DWORK522");
35071 ssSetDWorkWidth(S, 522, 1);
35072 ssSetDWorkDataType(S, 522, SS_UINT8);
35073
35074 /* '<S12>/LLACalibration2': DWORK3 */
35075 ssSetDWorkName(S, 523, "DWORK523");
35076 ssSetDWorkWidth(S, 523, 1);
35077 ssSetDWorkDataType(S, 523, SS_UINT8);
35078
35079 /* '<S32>/wowcmp1': DWORK1 */
35080 ssSetDWorkName(S, 524, "DWORK524");
35081 ssSetDWorkWidth(S, 524, 1);
35082 ssSetDWorkDataType(S, 524, SS_UINT8);
35083
35084 /* '<S32>/wowcmp1': DWORK2 */
35085 ssSetDWorkName(S, 525, "DWORK525");
35086 ssSetDWorkWidth(S, 525, 1);
35087 ssSetDWorkDataType(S, 525, SS_UINT8);
35088
35089 /* '<S32>/wowcmp1': DWORK3 */
35090 ssSetDWorkName(S, 526, "DWORK526");
35091 ssSetDWorkWidth(S, 526, 1);
35092 ssSetDWorkDataType(S, 526, SS_UINT8);
35093
35094 /* '<S322>/Taxi': DWORK1 */
35095 ssSetDWorkName(S, 527, "DWORK527");
35096 ssSetDWorkWidth(S, 527, 1);
35097 ssSetDWorkDataType(S, 527, SS_UINT8);
35098
35099 /* '<S322>/Taxi': DWORK2 */
35100 ssSetDWorkName(S, 528, "DWORK528");
35101 ssSetDWorkWidth(S, 528, 1);
35102 ssSetDWorkDataType(S, 528, SS_UINT8);
35103
35104 /* '<S322>/Taxi': DWORK3 */
35105 ssSetDWorkName(S, 529, "DWORK529");
35106 ssSetDWorkWidth(S, 529, 1);
35107 ssSetDWorkDataType(S, 529, SS_UINT8);
35108
35109 /* '<S322>/Taxi': DWORK4 */
35110 ssSetDWorkName(S, 530, "DWORK530");
35111 ssSetDWorkWidth(S, 530, 1);
35112 ssSetDWorkDataType(S, 530, SS_UINT8);
35113
35114 /* '<S322>/Taxi': DWORK5 */
35115 ssSetDWorkName(S, 531, "DWORK531");
35116 ssSetDWorkWidth(S, 531, 1);
35117 ssSetDWorkDataType(S, 531, SS_UINT8);
35118
35119 /* '<S316>/Landing2': DWORK1 */
35120 ssSetDWorkName(S, 532, "DWORK532");
35121 ssSetDWorkWidth(S, 532, 1);
35122 ssSetDWorkDataType(S, 532, SS_UINT8);
35123
35124 /* '<S316>/Landing2': DWORK2 */
35125 ssSetDWorkName(S, 533, "DWORK533");
35126 ssSetDWorkWidth(S, 533, 1);
35127 ssSetDWorkDataType(S, 533, SS_UINT8);
35128
35129 /* '<S316>/Landing2': DWORK3 */
35130 ssSetDWorkName(S, 534, "DWORK534");
35131 ssSetDWorkWidth(S, 534, 1);
35132 ssSetDWorkDataType(S, 534, SS_UINT8);
35133
35134 /* '<S316>/Landing2': DWORK4 */
35135 ssSetDWorkName(S, 535, "DWORK535");
35136 ssSetDWorkWidth(S, 535, 1);
35137 ssSetDWorkDataType(S, 535, SS_UINT8);
35138
35139 /* '<S316>/Landing2': DWORK5 */
35140 ssSetDWorkName(S, 536, "DWORK536");
35141 ssSetDWorkWidth(S, 536, 1);
35142 ssSetDWorkDataType(S, 536, SS_UINT8);
35143
35144 /* '<S316>/Landing2': DWORK6 */
35145 ssSetDWorkName(S, 537, "DWORK537");
35146 ssSetDWorkWidth(S, 537, 1);
35147 ssSetDWorkDataType(S, 537, SS_UINT8);
35148
35149 /* '<S316>/Landing2': DWORK7 */
35150 ssSetDWorkName(S, 538, "DWORK538");
35151 ssSetDWorkWidth(S, 538, 1);
35152 ssSetDWorkDataType(S, 538, SS_UINT8);
35153
35154 /* '<S316>/Landing2': DWORK8 */
35155 ssSetDWorkName(S, 539, "DWORK539");
35156 ssSetDWorkWidth(S, 539, 1);
35157 ssSetDWorkDataType(S, 539, SS_UINT8);
35158
35159 /* '<S316>/Landing2': DWORK9 */
35160 ssSetDWorkName(S, 540, "DWORK540");
35161 ssSetDWorkWidth(S, 540, 1);
35162 ssSetDWorkDataType(S, 540, SS_UINT8);
35163
35164 /* '<S316>/Landing2': DWORK10 */
35165 ssSetDWorkName(S, 541, "DWORK541");
35166 ssSetDWorkWidth(S, 541, 1);
35167 ssSetDWorkDataType(S, 541, SS_UINT8);
35168
35169 /* '<S316>/Landing2': DWORK11 */
35170 ssSetDWorkName(S, 542, "DWORK542");
35171 ssSetDWorkWidth(S, 542, 1);
35172 ssSetDWorkDataType(S, 542, SS_UINT8);
35173
35174 /* '<S316>/Landing2': DWORK12 */
35175 ssSetDWorkName(S, 543, "DWORK543");
35176 ssSetDWorkWidth(S, 543, 1);
35177 ssSetDWorkDataType(S, 543, SS_UINT8);
35178
35179 /* '<S316>/Landing2': DWORK13 */
35180 ssSetDWorkName(S, 544, "DWORK544");
35181 ssSetDWorkWidth(S, 544, 1);
35182 ssSetDWorkDataType(S, 544, SS_UINT8);
35183
35184 /* '<S316>/Landing2': DWORK14 */
35185 ssSetDWorkName(S, 545, "DWORK545");
35186 ssSetDWorkWidth(S, 545, 1);
35187 ssSetDWorkDataType(S, 545, SS_UINT8);
35188
35189 /* '<S316>/Landing2': DWORK15 */
35190 ssSetDWorkName(S, 546, "DWORK546");
35191 ssSetDWorkWidth(S, 546, 1);
35192 ssSetDWorkDataType(S, 546, SS_UINT8);
35193
35194 /* '<S313>/Approach2': DWORK1 */
35195 ssSetDWorkName(S, 547, "DWORK547");
35196 ssSetDWorkWidth(S, 547, 1);
35197 ssSetDWorkDataType(S, 547, SS_UINT8);
35198
35199 /* '<S313>/Approach2': DWORK2 */
35200 ssSetDWorkName(S, 548, "DWORK548");
35201 ssSetDWorkWidth(S, 548, 1);
35202 ssSetDWorkDataType(S, 548, SS_UINT8);
35203
35204 /* '<S313>/Approach2': DWORK3 */
35205 ssSetDWorkName(S, 549, "DWORK549");
35206 ssSetDWorkWidth(S, 549, 1);
35207 ssSetDWorkDataType(S, 549, SS_UINT8);
35208
35209 /* '<S313>/Approach2': DWORK4 */
35210 ssSetDWorkName(S, 550, "DWORK550");
35211 ssSetDWorkWidth(S, 550, 1);
35212 ssSetDWorkDataType(S, 550, SS_UINT8);
35213
35214 /* '<S313>/Approach2': DWORK5 */
35215 ssSetDWorkName(S, 551, "DWORK551");
35216 ssSetDWorkWidth(S, 551, 1);
35217 ssSetDWorkDataType(S, 551, SS_UINT8);
35218
35219 /* '<S313>/Approach2': DWORK6 */
35220 ssSetDWorkName(S, 552, "DWORK552");
35221 ssSetDWorkWidth(S, 552, 1);
35222 ssSetDWorkDataType(S, 552, SS_UINT8);
35223
35224 /* '<S313>/Approach2': DWORK7 */
35225 ssSetDWorkName(S, 553, "DWORK553");
35226 ssSetDWorkWidth(S, 553, 1);
35227 ssSetDWorkDataType(S, 553, SS_UINT8);
35228
35229 /* '<S313>/Approach2': DWORK8 */
35230 ssSetDWorkName(S, 554, "DWORK554");
35231 ssSetDWorkWidth(S, 554, 1);
35232 ssSetDWorkDataType(S, 554, SS_UINT8);
35233
35234 /* '<S313>/Approach2': DWORK9 */
35235 ssSetDWorkName(S, 555, "DWORK555");
35236 ssSetDWorkWidth(S, 555, 1);
35237 ssSetDWorkDataType(S, 555, SS_UINT8);
35238
35239 /* '<S313>/Approach2': DWORK10 */
35240 ssSetDWorkName(S, 556, "DWORK556");
35241 ssSetDWorkWidth(S, 556, 1);
35242 ssSetDWorkDataType(S, 556, SS_UINT8);
35243
35244 /* '<S315>/Cruise': DWORK1 */
35245 ssSetDWorkName(S, 557, "DWORK557");
35246 ssSetDWorkWidth(S, 557, 1);
35247 ssSetDWorkDataType(S, 557, SS_UINT8);
35248
35249 /* '<S315>/Cruise': DWORK2 */
35250 ssSetDWorkName(S, 558, "DWORK558");
35251 ssSetDWorkWidth(S, 558, 1);
35252 ssSetDWorkDataType(S, 558, SS_UINT8);
35253
35254 /* '<S315>/Cruise': DWORK3 */
35255 ssSetDWorkName(S, 559, "DWORK559");
35256 ssSetDWorkWidth(S, 559, 1);
35257 ssSetDWorkDataType(S, 559, SS_UINT8);
35258
35259 /* '<S315>/Cruise': DWORK4 */
35260 ssSetDWorkName(S, 560, "DWORK560");
35261 ssSetDWorkWidth(S, 560, 1);
35262 ssSetDWorkDataType(S, 560, SS_UINT8);
35263
35264 /* '<S315>/Cruise': DWORK5 */
35265 ssSetDWorkName(S, 561, "DWORK561");
35266 ssSetDWorkWidth(S, 561, 1);
35267 ssSetDWorkDataType(S, 561, SS_UINT8);
35268
35269 /* '<S315>/Cruise': DWORK6 */
35270 ssSetDWorkName(S, 562, "DWORK562");
35271 ssSetDWorkWidth(S, 562, 1);
35272 ssSetDWorkDataType(S, 562, SS_UINT8);
35273
35274 /* '<S315>/Cruise': DWORK7 */
35275 ssSetDWorkName(S, 563, "DWORK563");
35276 ssSetDWorkWidth(S, 563, 1);
35277 ssSetDWorkDataType(S, 563, SS_UINT8);
35278
35279 /* '<S315>/Cruise': DWORK8 */
35280 ssSetDWorkName(S, 564, "DWORK564");
35281 ssSetDWorkWidth(S, 564, 1);
35282 ssSetDWorkDataType(S, 564, SS_UINT8);
35283
35284 /* '<S315>/Cruise': DWORK9 */
35285 ssSetDWorkName(S, 565, "DWORK565");
35286 ssSetDWorkWidth(S, 565, 1);
35287 ssSetDWorkDataType(S, 565, SS_UINT8);
35288
35289 /* '<S315>/Cruise': DWORK10 */
35290 ssSetDWorkName(S, 566, "DWORK566");
35291 ssSetDWorkWidth(S, 566, 1);
35292 ssSetDWorkDataType(S, 566, SS_UINT8);
35293
35294 /* '<S314>/Climb2': DWORK1 */
35295 ssSetDWorkName(S, 567, "DWORK567");
35296 ssSetDWorkWidth(S, 567, 1);
35297 ssSetDWorkDataType(S, 567, SS_UINT8);
35298
35299 /* '<S314>/Climb2': DWORK2 */
35300 ssSetDWorkName(S, 568, "DWORK568");
35301 ssSetDWorkWidth(S, 568, 1);
35302 ssSetDWorkDataType(S, 568, SS_UINT8);
35303
35304 /* '<S314>/Climb2': DWORK3 */
35305 ssSetDWorkName(S, 569, "DWORK569");
35306 ssSetDWorkWidth(S, 569, 1);
35307 ssSetDWorkDataType(S, 569, SS_UINT8);
35308
35309 /* '<S314>/Climb2': DWORK4 */
35310 ssSetDWorkName(S, 570, "DWORK570");
35311 ssSetDWorkWidth(S, 570, 1);
35312 ssSetDWorkDataType(S, 570, SS_UINT8);
35313
35314 /* '<S314>/Climb2': DWORK5 */
35315 ssSetDWorkName(S, 571, "DWORK571");
35316 ssSetDWorkWidth(S, 571, 1);
35317 ssSetDWorkDataType(S, 571, SS_UINT8);
35318
35319 /* '<S314>/Climb2': DWORK6 */
35320 ssSetDWorkName(S, 572, "DWORK572");
35321 ssSetDWorkWidth(S, 572, 1);
35322 ssSetDWorkDataType(S, 572, SS_UINT8);
35323
35324 /* '<S314>/Climb2': DWORK7 */
35325 ssSetDWorkName(S, 573, "DWORK573");
35326 ssSetDWorkWidth(S, 573, 1);
35327 ssSetDWorkDataType(S, 573, SS_UINT8);
35328
35329 /* '<S314>/Climb2': DWORK8 */
35330 ssSetDWorkName(S, 574, "DWORK574");
35331 ssSetDWorkWidth(S, 574, 1);
35332 ssSetDWorkDataType(S, 574, SS_UINT8);
35333
35334 /* '<S314>/Climb2': DWORK9 */
35335 ssSetDWorkName(S, 575, "DWORK575");
35336 ssSetDWorkWidth(S, 575, 1);
35337 ssSetDWorkDataType(S, 575, SS_UINT8);
35338
35339 /* '<S321>/Takeoff': DWORK1 */
35340 ssSetDWorkName(S, 576, "DWORK576");
35341 ssSetDWorkWidth(S, 576, 1);
35342 ssSetDWorkDataType(S, 576, SS_UINT8);
35343
35344 /* '<S321>/Takeoff': DWORK2 */
35345 ssSetDWorkName(S, 577, "DWORK577");
35346 ssSetDWorkWidth(S, 577, 1);
35347 ssSetDWorkDataType(S, 577, SS_UINT8);
35348
35349 /* '<S321>/Takeoff': DWORK3 */
35350 ssSetDWorkName(S, 578, "DWORK578");
35351 ssSetDWorkWidth(S, 578, 1);
35352 ssSetDWorkDataType(S, 578, SS_UINT8);
35353
35354 /* '<S321>/Takeoff': DWORK4 */
35355 ssSetDWorkName(S, 579, "DWORK579");
35356 ssSetDWorkWidth(S, 579, 1);
35357 ssSetDWorkDataType(S, 579, SS_UINT8);
35358
35359 /* '<S319>/Pre_Takeoff': DWORK1 */
35360 ssSetDWorkName(S, 580, "DWORK580");
35361 ssSetDWorkWidth(S, 580, 1);
35362 ssSetDWorkDataType(S, 580, SS_UINT8);
35363
35364 /* '<S319>/Pre_Takeoff': DWORK2 */
35365 ssSetDWorkName(S, 581, "DWORK581");
35366 ssSetDWorkWidth(S, 581, 1);
35367 ssSetDWorkDataType(S, 581, SS_UINT8);
35368
35369 /* '<S319>/Pre_Takeoff': DWORK3 */
35370 ssSetDWorkName(S, 582, "DWORK582");
35371 ssSetDWorkWidth(S, 582, 1);
35372 ssSetDWorkDataType(S, 582, SS_UINT8);
35373
35374 /* '<S29>/LLACalibration2': DWORK1 */
35375 ssSetDWorkName(S, 583, "DWORK583");
35376 ssSetDWorkWidth(S, 583, 1);
35377 ssSetDWorkDataType(S, 583, SS_UINT8);
35378
35379 /* '<S29>/LLACalibration2': DWORK2 */
35380 ssSetDWorkName(S, 584, "DWORK584");
35381 ssSetDWorkWidth(S, 584, 1);
35382 ssSetDWorkDataType(S, 584, SS_UINT8);
35383
35384 /* '<S29>/LLACalibration2': DWORK3 */
35385 ssSetDWorkName(S, 585, "DWORK585");
35386 ssSetDWorkWidth(S, 585, 1);
35387 ssSetDWorkDataType(S, 585, SS_UINT8);
35388
35389 /* '<S24>/zeros2': DWORK1 */
35390 ssSetDWorkName(S, 586, "DWORK586");
35391 ssSetDWorkWidth(S, 586, 1);
35392 ssSetDWorkDataType(S, 586, SS_UINT8);
35393
35394 /* '<S24>/zeros2': DWORK2 */
35395 ssSetDWorkName(S, 587, "DWORK587");
35396 ssSetDWorkWidth(S, 587, 1);
35397 ssSetDWorkDataType(S, 587, SS_UINT8);
35398
35399 /* '<S299>/AirLineUpdateSucModel': DWORK1 */
35400 ssSetDWorkName(S, 588, "DWORK588");
35401 ssSetDWorkWidth(S, 588, 1);
35402 ssSetDWorkDataType(S, 588, SS_UINT8);
35403
35404 /* '<S299>/AirLineUpdateSucModel': DWORK2 */
35405 ssSetDWorkName(S, 589, "DWORK589");
35406 ssSetDWorkWidth(S, 589, 1);
35407 ssSetDWorkDataType(S, 589, SS_UINT8);
35408
35409 /* '<S299>/AirLineUpdateSucModel': DWORK3 */
35410 ssSetDWorkName(S, 590, "DWORK590");
35411 ssSetDWorkWidth(S, 590, 1);
35412 ssSetDWorkDataType(S, 590, SS_UINT8);
35413
35414 /* '<S21>/delayblock7': DWORK1 */
35415 ssSetDWorkName(S, 591, "DWORK591");
35416 ssSetDWorkWidth(S, 591, 1);
35417 ssSetDWorkDataType(S, 591, SS_UINT8);
35418
35419 /* '<S21>/delayblock7': DWORK2 */
35420 ssSetDWorkName(S, 592, "DWORK592");
35421 ssSetDWorkWidth(S, 592, 1);
35422 ssSetDWorkDataType(S, 592, SS_UINT8);
35423
35424 /* '<S21>/delayblock7': DWORK3 */
35425 ssSetDWorkName(S, 593, "DWORK593");
35426 ssSetDWorkWidth(S, 593, 1);
35427 ssSetDWorkDataType(S, 593, SS_UINT8);
35428
35429 /* '<S21>/delayblock6': DWORK1 */
35430 ssSetDWorkName(S, 594, "DWORK594");
35431 ssSetDWorkWidth(S, 594, 1);
35432 ssSetDWorkDataType(S, 594, SS_UINT8);
35433
35434 /* '<S21>/delayblock6': DWORK2 */
35435 ssSetDWorkName(S, 595, "DWORK595");
35436 ssSetDWorkWidth(S, 595, 1);
35437 ssSetDWorkDataType(S, 595, SS_UINT8);
35438
35439 /* '<S21>/delayblock6': DWORK3 */
35440 ssSetDWorkName(S, 596, "DWORK596");
35441 ssSetDWorkWidth(S, 596, 1);
35442 ssSetDWorkDataType(S, 596, SS_UINT8);
35443
35444 /* '<S21>/delayblock5': DWORK1 */
35445 ssSetDWorkName(S, 597, "DWORK597");
35446 ssSetDWorkWidth(S, 597, 1);
35447 ssSetDWorkDataType(S, 597, SS_UINT8);
35448
35449 /* '<S21>/delayblock5': DWORK2 */
35450 ssSetDWorkName(S, 598, "DWORK598");
35451 ssSetDWorkWidth(S, 598, 1);
35452 ssSetDWorkDataType(S, 598, SS_UINT8);
35453
35454 /* '<S21>/delayblock5': DWORK3 */
35455 ssSetDWorkName(S, 599, "DWORK599");
35456 ssSetDWorkWidth(S, 599, 1);
35457 ssSetDWorkDataType(S, 599, SS_UINT8);
35458
35459 /* '<S21>/delayblock4': DWORK1 */
35460 ssSetDWorkName(S, 600, "DWORK600");
35461 ssSetDWorkWidth(S, 600, 1);
35462 ssSetDWorkDataType(S, 600, SS_UINT8);
35463
35464 /* '<S21>/delayblock4': DWORK2 */
35465 ssSetDWorkName(S, 601, "DWORK601");
35466 ssSetDWorkWidth(S, 601, 1);
35467 ssSetDWorkDataType(S, 601, SS_UINT8);
35468
35469 /* '<S21>/delayblock4': DWORK3 */
35470 ssSetDWorkName(S, 602, "DWORK602");
35471 ssSetDWorkWidth(S, 602, 1);
35472 ssSetDWorkDataType(S, 602, SS_UINT8);
35473
35474 /* '<S21>/delayblock3': DWORK1 */
35475 ssSetDWorkName(S, 603, "DWORK603");
35476 ssSetDWorkWidth(S, 603, 1);
35477 ssSetDWorkDataType(S, 603, SS_UINT8);
35478
35479 /* '<S21>/delayblock3': DWORK2 */
35480 ssSetDWorkName(S, 604, "DWORK604");
35481 ssSetDWorkWidth(S, 604, 1);
35482 ssSetDWorkDataType(S, 604, SS_UINT8);
35483
35484 /* '<S21>/delayblock3': DWORK3 */
35485 ssSetDWorkName(S, 605, "DWORK605");
35486 ssSetDWorkWidth(S, 605, 1);
35487 ssSetDWorkDataType(S, 605, SS_UINT8);
35488
35489 /* '<S21>/delayblock2': DWORK1 */
35490 ssSetDWorkName(S, 606, "DWORK606");
35491 ssSetDWorkWidth(S, 606, 1);
35492 ssSetDWorkDataType(S, 606, SS_UINT8);
35493
35494 /* '<S21>/delayblock2': DWORK2 */
35495 ssSetDWorkName(S, 607, "DWORK607");
35496 ssSetDWorkWidth(S, 607, 1);
35497 ssSetDWorkDataType(S, 607, SS_UINT8);
35498
35499 /* '<S21>/delayblock2': DWORK3 */
35500 ssSetDWorkName(S, 608, "DWORK608");
35501 ssSetDWorkWidth(S, 608, 1);
35502 ssSetDWorkDataType(S, 608, SS_UINT8);
35503
35504 /* '<S21>/delayblock1': DWORK1 */
35505 ssSetDWorkName(S, 609, "DWORK609");
35506 ssSetDWorkWidth(S, 609, 1);
35507 ssSetDWorkDataType(S, 609, SS_UINT8);
35508
35509 /* '<S21>/delayblock1': DWORK2 */
35510 ssSetDWorkName(S, 610, "DWORK610");
35511 ssSetDWorkWidth(S, 610, 1);
35512 ssSetDWorkDataType(S, 610, SS_UINT8);
35513
35514 /* '<S21>/delayblock1': DWORK3 */
35515 ssSetDWorkName(S, 611, "DWORK611");
35516 ssSetDWorkWidth(S, 611, 1);
35517 ssSetDWorkDataType(S, 611, SS_UINT8);
35518
35519 /* '<S9>/Memory1': PreviousInput */
35520 ssSetDWorkName(S, 612, "DWORK612");
35521 ssSetDWorkWidth(S, 612, 1);
35522 ssSetDWorkDataType(S, 612, SS_BOOLEAN);
35523
35524 /* '<S299>/Enabled Subsystem': MODE */
35525 ssSetDWorkName(S, 613, "DWORK613");
35526 ssSetDWorkWidth(S, 613, 1);
35527 ssSetDWorkDataType(S, 613, SS_BOOLEAN);
35528
35529 /* '<S15>/Subsystem3': MODE */
35530 ssSetDWorkName(S, 614, "DWORK614");
35531 ssSetDWorkWidth(S, 614, 1);
35532 ssSetDWorkDataType(S, 614, SS_BOOLEAN);
35533
35534 /* '<S727>/Xcmpr_prev': PreviousInput */
35535 ssSetDWorkName(S, 615, "DWORK615");
35536 ssSetDWorkWidth(S, 615, 1);
35537
35538 /* '<S717>/Scope1': PWORK */
35539 ssSetDWorkName(S, 616, "DWORK616");
35540 ssSetDWorkWidth(S, 616, 1);
35541 ssSetDWorkDataType(S, 616, SS_POINTER);
35542
35543 /* '<S717>/Scope2': PWORK */
35544 ssSetDWorkName(S, 617, "DWORK617");
35545 ssSetDWorkWidth(S, 617, 1);
35546 ssSetDWorkDataType(S, 617, SS_POINTER);
35547
35548 /* '<S717>/Scope5': PWORK */
35549 ssSetDWorkName(S, 618, "DWORK618");
35550 ssSetDWorkWidth(S, 618, 1);
35551 ssSetDWorkDataType(S, 618, SS_POINTER);
35552
35553 /* '<S720>/Scope1': PWORK */
35554 ssSetDWorkName(S, 619, "DWORK619");
35555 ssSetDWorkWidth(S, 619, 1);
35556 ssSetDWorkDataType(S, 619, SS_POINTER);
35557
35558 /* '<S720>/Scope10': PWORK */
35559 ssSetDWorkName(S, 620, "DWORK620");
35560 ssSetDWorkWidth(S, 620, 1);
35561 ssSetDWorkDataType(S, 620, SS_POINTER);
35562
35563 /* '<S720>/Scope11': PWORK */
35564 ssSetDWorkName(S, 621, "DWORK621");
35565 ssSetDWorkWidth(S, 621, 1);
35566 ssSetDWorkDataType(S, 621, SS_POINTER);
35567
35568 /* '<S720>/Scope7': PWORK */
35569 ssSetDWorkName(S, 622, "DWORK622");
35570 ssSetDWorkWidth(S, 622, 1);
35571 ssSetDWorkDataType(S, 622, SS_POINTER);
35572
35573 /* '<S621>/GenerateGroundFM': MODE */
35574 ssSetDWorkName(S, 623, "DWORK623");
35575 ssSetDWorkWidth(S, 623, 1);
35576 ssSetDWorkDataType(S, 623, SS_BOOLEAN);
35577
35578 /* '<S693>/Xcmpr_prev': PreviousInput */
35579 ssSetDWorkName(S, 624, "DWORK624");
35580 ssSetDWorkWidth(S, 624, 1);
35581
35582 /* '<S686>/Scope1': PWORK */
35583 ssSetDWorkName(S, 625, "DWORK625");
35584 ssSetDWorkWidth(S, 625, 1);
35585 ssSetDWorkDataType(S, 625, SS_POINTER);
35586
35587 /* '<S686>/Scope5': PWORK */
35588 ssSetDWorkName(S, 626, "DWORK626");
35589 ssSetDWorkWidth(S, 626, 1);
35590 ssSetDWorkDataType(S, 626, SS_POINTER);
35591
35592 /* '<S688>/Scope8': PWORK */
35593 ssSetDWorkName(S, 627, "DWORK627");
35594 ssSetDWorkWidth(S, 627, 1);
35595 ssSetDWorkDataType(S, 627, SS_POINTER);
35596
35597 /* '<S620>/GenerateGroundFM': MODE */
35598 ssSetDWorkName(S, 628, "DWORK628");
35599 ssSetDWorkWidth(S, 628, 1);
35600 ssSetDWorkDataType(S, 628, SS_BOOLEAN);
35601
35602 /* '<S660>/Xcmpr_prev': PreviousInput */
35603 ssSetDWorkName(S, 629, "DWORK629");
35604 ssSetDWorkWidth(S, 629, 1);
35605
35606 /* '<S618>/Scope1': PWORK */
35607 ssSetDWorkName(S, 630, "DWORK630");
35608 ssSetDWorkWidth(S, 630, 1);
35609 ssSetDWorkDataType(S, 630, SS_POINTER);
35610
35611 /* '<S618>/Scope2': PWORK */
35612 ssSetDWorkName(S, 631, "DWORK631");
35613 ssSetDWorkWidth(S, 631, 1);
35614 ssSetDWorkDataType(S, 631, SS_POINTER);
35615
35616 /* '<S651>/Scope1': PWORK */
35617 ssSetDWorkName(S, 632, "DWORK632");
35618 ssSetDWorkWidth(S, 632, 1);
35619 ssSetDWorkDataType(S, 632, SS_POINTER);
35620
35621 /* '<S651>/Scope2': PWORK */
35622 ssSetDWorkName(S, 633, "DWORK633");
35623 ssSetDWorkWidth(S, 633, 1);
35624 ssSetDWorkDataType(S, 633, SS_POINTER);
35625
35626 /* '<S651>/Scope3': PWORK */
35627 ssSetDWorkName(S, 634, "DWORK634");
35628 ssSetDWorkWidth(S, 634, 1);
35629 ssSetDWorkDataType(S, 634, SS_POINTER);
35630
35631 /* '<S651>/Scope5': PWORK */
35632 ssSetDWorkName(S, 635, "DWORK635");
35633 ssSetDWorkWidth(S, 635, 1);
35634 ssSetDWorkDataType(S, 635, SS_POINTER);
35635
35636 /* '<S654>/Scope1': PWORK */
35637 ssSetDWorkName(S, 636, "DWORK636");
35638 ssSetDWorkWidth(S, 636, 1);
35639 ssSetDWorkDataType(S, 636, SS_POINTER);
35640
35641 /* '<S654>/Scope10': PWORK */
35642 ssSetDWorkName(S, 637, "DWORK637");
35643 ssSetDWorkWidth(S, 637, 1);
35644 ssSetDWorkDataType(S, 637, SS_POINTER);
35645
35646 /* '<S654>/Scope11': PWORK */
35647 ssSetDWorkName(S, 638, "DWORK638");
35648 ssSetDWorkWidth(S, 638, 1);
35649 ssSetDWorkDataType(S, 638, SS_POINTER);
35650
35651 /* '<S654>/Scope2': PWORK */
35652 ssSetDWorkName(S, 639, "DWORK639");
35653 ssSetDWorkWidth(S, 639, 1);
35654 ssSetDWorkDataType(S, 639, SS_POINTER);
35655
35656 /* '<S618>/GenerateGroundFM': MODE */
35657 ssSetDWorkName(S, 640, "DWORK640");
35658 ssSetDWorkWidth(S, 640, 1);
35659 ssSetDWorkDataType(S, 640, SS_BOOLEAN);
35660
35661 /* Tunable Parameters */
35662 ssSetNumSFcnParams(S, 0);
35663
35664 /* Number of expected parameters */
35665#if defined(MATLAB_MEX_FILE)
35666
35667 if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
35668
35669#if defined(MDL_CHECK_PARAMETERS)
35670
35671 mdlCheckParameters(S);
35672
35673#endif /* MDL_CHECK_PARAMETERS */
35674
35675 if (ssGetErrorStatus(S) != (NULL) ) {
35676 return;
35677 }
35678 } else {
35679 return; /* Parameter mismatch will be reported by Simulink */
35680 }
35681
35682#endif /* MATLAB_MEX_FILE */
35683
35684 /* Options */
35685 ssSetOptions(S, (SS_OPTION_RUNTIME_EXCEPTION_FREE_CODE ));
35686
35687#if SS_SFCN_FOR_SIM
35688
35689 {
35690 ssSupportsMultipleExecInstances(S, false);
35691 ssRegisterMsgForNotSupportingMultiExecInst(S,
35692 "<diag_root><diag id=\"Simulink:blocks:ImplicitIterSS_TestPointNotSupportedInside\"><arguments><arg type=\"numeric\">4</arg><arg type=\"encoded\">QQBGAEMAUwBfAE0ATwBEAEUATAAxAC8AQQBGAEMAUwBfAE0ATwBEAEUATAAxAC8ARABvAHcAbgBsAGkAbgBrADEALwBCAHUAcwAgAFMAZQBsAGUAYwB0AG8AcgAAAA==</arg><arg type=\"encoded\">PABfAF8AaQBpAFMAUwBfAF8APgA8AC8AXwBfAGkAaQBTAFMAXwBfAD4AAAA=</arg><arg type=\"encoded\">PABfAF8AaQB0AGUAcgBCAGwAawBfAF8APgA8AC8AXwBfAGkAdABlAHIAQgBsAGsAXwBfAD4AAAA=</arg></arguments></diag>\n</diag_root>");
35693 ssHasStateInsideForEachSS(S, false);
35694 }
35695
35696#endif
35697
35698}
35699
35700/* Function to initialize sample times. */
35701static void mdlInitializeSampleTimes(SimStruct *S)
35702{
35703 /* task periods */
35704 ssSetSampleTime(S, 0, 0.0);
35705 ssSetSampleTime(S, 1, 0.005);
35706
35707 /* task offsets */
35708 ssSetOffsetTime(S, 0, 0.0);
35709 ssSetOffsetTime(S, 1, 0.0);
35710}
35711
35712#if defined(MATLAB_MEX_FILE)
35713# include "fixedpoint.c"
35714# include "simulink.c"
35715#else
35716# undef S_FUNCTION_NAME
35717# define S_FUNCTION_NAME AFCS_MODEL1_sf
35718# include "cg_sfun.h"
35719#endif /* defined(MATLAB_MEX_FILE) */
35720